def controlMotor(xmotor): global control global motor global terminate xcontrol = control med = mcp.read_adc(ICPIN[xcontrol]) mh = Adafruit_MotorHAT(addr=0x60) global mym if xmotor == "1": mym = mh.getMotor(PIN[xmotor]) elif xmotor == "2": mym = mh.getStepper(200, PIN[xmotor]) else: return #Speed [0,255] mym.setSpeed(150) try: while True: if control != xcontrol or motor != xmotor or terminate: turnOffMotors() return value = mcp.read_adc(ICPIN[xcontrol]) if value < med - (med - MIN[xcontrol])/ DEADZONE[xcontrol]: value = (int)((float)(value - med) / (med - MIN[xcontrol]) * SCALE) elif value > med + (MAX[xcontrol] - med)/ DEADZONE[xcontrol]: value = (int)((float)(value - med) / (MAX[xcontrol] - med) * SCALE) else: value = 0 MOTORS[xmotor](value) time.sleep( .05 ) except KeyError: print "Error"
class cycle(): global SERVO_MAX global SERVO_MIN global current_us global servo global mh global stepper def __init__(self): self.mh = Adafruit_MotorHAT() self.stepper = self.mh.getStepper(200,1) self.stepper.setSpeed(10) self.servo = PWM.Servo() self.SERVO_MAX = 2400 self.SERVO_MIN = 600 self.current_us = 2300 self.servo.set_servo(18,self.current_us) time.sleep(1) def move_stepper_time(self,time_min): s_per_s = 0.0125 time_sec = time_min * 60 self.stepper.step(int(time_sec/s_per_s), Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) def move_stepper_steps(self,steps): self.stepper.step(steps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) def move_servo_angle(self, angle, speed): angle_us = ((180-angle)*10)+self.SERVO_MIN self.move_servo(angle_us, speed) def move_servo(self, set_us,speed): if(set_us%10 != 0) and (set_us != 0): print("Invalid servo pulse") self.quit() if(set_us > self.SERVO_MAX) or (set_us < self.SERVO_MIN): self.flip() else: #get us? next_us = self.current_us + speed self.current_us = set_us while(next_us != set_us): if(set_us>next_us): self.servo.set_servo(18,next_us) print(next_us) next_us = next_us + speed if(set_us<next_us): self.servo.set_servo(18,next_us) print(next_us) next_us = next_us - speed time.sleep(int(0.05*speed)) def quit(self): self.servo.stop_servo(18) self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) sys.exit() def flip(self): print("TODO :: flipping..")
class Motor: def __init__(self): self.mh = Adafruit_MotorHAT(addr= 0x60) self.stepper = self.mh.getStepper(200, 1) def turnoffmotors(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) def microstep(self, numsteps, speed): self.stepper.setSpeed(speed) self.stepper.step(numsteps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.MICROSTEP) def interleave(self, numsteps, speed): self.stepper.setSpeed(speed) self.stepper.step(numsteps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) def revinterleave(self, numsteps, speed): self.stepper.setSpeed(speed) self.stepper.step(numsteps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) def revmicrostep(self, numsteps, speed): self.stepper.setSpeed(speed) self.stepper.step(numsteps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) def singlestep(self, numsteps, speed): self.stepper.setSpeed(speed) self.stepper.step(numsteps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) def onestep(self, direction, style): self.stepper.oneStep(direction, style)
class StepperHat(object): DIRS = [Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.FORWARD] #backwards is towrad the drum HIT_ZONE = -8 # where drum is TAP_LINE = 0 # rreturn here if you hve more beats to hit RETREAT = 6 # as far back as we go HIT_SPEED = 180 RETREAT_SPEED = 100 def __init__(self): self.mh = Adafruit_MotorHAT() self.stepper = self.mh.getStepper(revstep, 1) #self.dir_sel = 0 #driection select self.dir_sel = 1 self.coming_beats = 0 self.position = -8 #net count of taken steps # recommended for auto-disabling motors on shutdown! def turnOffMotors(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) #stepper's callback def thump(self, data): if self.position > StepperHat.TAP_LINE: self.stepper.setSpeed(StepperHat.HIT_SPEED) self.dir_sel = 0 self.coming_beats += 1 def run_stepper(self): if (self.position >= StepperHat.RETREAT and not self.coming_beats): # or (not self.coming_beats and self.position >= StepperHat.TAP_LINE) return False else: if self.position >= StepperHat.RETREAT: self.stepper.setSpeed(StepperHat.HIT_SPEED) self.dir_sel = 0 elif self.position < StepperHat.HIT_ZONE: self.stepper.setSpeed(StepperHat.RETREAT_SPEED) self.dir_sel = 1 self.coming_beats -= 1 elif self.position >= StepperHat.TAP_LINE and self.coming_beats: # and self.coming_beats>0: #already checking that self.stepper.setSpeed(StepperHat.HIT_SPEED) self.dir_sel = 0 self.stepper.oneStep(StepperHat.DIRS[self.dir_sel], Adafruit_MotorHAT.DOUBLE) self.position += (1 if self.dir_sel else -1) print(self.position, self.dir_sel, self.coming_beats) return self.dir_sel
def feed(): # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() myStepper = mh.getStepper(200, 1) # 200 steps/rev, lower motor port #1 myStepper.setSpeed(480) myStepper.step(200, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE)
class Motor: def __init__(self): self.mh = Adafruit_MotorHAT(addr=0x60) self.rearMotor1 = self.mh.getMotor(1) self.rearMotor2 = self.mh.getMotor(2) self.servoMotor = self.mh.getStepper(100,3) def turnOffMotors(): self.rearMotor1.run(Adafruit_MotorHAT.RELEASE) self.rearMotor2.run(Adafruit_MotorHAT.RELEASE) self.servoMotor.run(Adafruit_MotorHAT.RELEASE) self.servoMotor.setSpeed(20) def driveForward(): self.rearMotor1.setSpeed(150) self.rearMotor2.setSpeed(150) self.rearMotor1.run(Adafruit_MotorHAT.FORWARD) self.rearMotor2.run(Adafruit_MotorHAT.FORWARD) def driveBackward(): self.rearMotor1.setSpeed(150) self.rearMotor2.setSpeed(150) self.rearMotor1.run(Adafruit_MotorHAT.BACKWARD) self.rearMotor2.run(Adafruit_MotorHAT.BACKWARD) def turnLeft(): # Not a stepper motor? self.servoMotor.step(10, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) def turnRight(): self.servoMotor.step(10, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE)
def speedup(speedlevel): # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() myStepper = mh.getStepper(200, 2) # 200 steps/rev, lower motor port #2 myStepper.setSpeed(480) # Set speed print("Speed UP") speedlevel = speedlevel*float((35/3)) myStepper.step(int(speedlevel), Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE)
def __init__(self,motor,steps = 200,addr = 0x60): motorPort = MOTORS[motor] self.motorPort = motorPort self.steps = steps self.hatAddress = addr global MH MH = Adafruit_MotorHAT(addr) self.stepperMotor = MH.getStepper(steps, motorPort) self.stepperMotor.setSpeed(180)
def Forwards(self): # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() # 200 steps/rev, motor port #1 myStepper = mh.getStepper(200, 1) # step velocity to a fixed number. myStepper.setSpeed(100) #even though the real speed is 30 rpm aprox. myStepper.step(170, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE)
def speeddown(speedlevel): # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() myStepper = mh.getStepper(200, 2) # 200 steps/rev, lower motor port #2 myStepper.setSpeed(480) # Set speed print("Speed DOWN") #Made a new thing just for this! BIPOLAR_BACKWARD! speedlevel = speedlevel*float((35/3)) myStepper.step(int(speedlevel), Adafruit_MotorHAT.BIPOLAR_BACKWARD, Adafruit_MotorHAT.DOUBLE)
def __init__(self): """ setup the GPIO and motor hat for the Pi :return: BoxController """ GPIO.setmode(GPIO.BCM) GPIO.setup(self.SENSOR_OUT_PIN, GPIO.OUT) GPIO.setup(self.SENSOR_IN_PIN, GPIO.IN, pull_up_down=GPIO.PUD_DOWN) mh = Adafruit_MotorHAT(addr=0x60) self.stepper = mh.getStepper(200, 1) self.stepper.setSpeed(1000)
def test_motor(hat, motor): print hat print motor global TYPE hat = int(hat, 16) mh = Adafruit_MotorHAT(addr = hat) motor_number = motor myStepper = mh.getStepper(200, motor_number) # 200 steps/rev, motor port #1 myStepper.setSpeed(120) # 30 RPM myStepper.step(STEPS, Adafruit_MotorHAT.FORWARD, TYPE) myStepper.step(STEPS, Adafruit_MotorHAT.BACKWARD, TYPE)
def angledownY(degrees): #13 revs (2600 steps) per 5 degrees (expect a value from 0 to 30 degrees #2.6 revs (520 steps) per 1 degree stepnumber = int(degrees * 520) # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr=0x61) myStepper = mh.getStepper(200, 2) # 200 steps/rev, upper motor port #2 (y-motor) myStepper.setSpeed(480) # Set speed print("Y Angle DOWN") myStepper.step(stepnumber, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE)
def _setup(motor_num: int = 1): mh = MotorHAT() # recommended for auto-disabling motors on shutdown! def turn_off_motors(): mh.getMotor(1).run(MotorHAT.RELEASE) mh.getMotor(2).run(MotorHAT.RELEASE) mh.getMotor(3).run(MotorHAT.RELEASE) mh.getMotor(4).run(MotorHAT.RELEASE) atexit.register(turn_off_motors) stepper = mh.getStepper(200, motor_num) # stepper.setSpeed(30) return stepper
class AutoGaugeWidget(widget.Widget): """Automotive gauge stepper motor. Small dual-coil stepper that can travel ~315 degrees with 600 steps. Send a numeric value from 0-100 to move the dial to the specified percent along its entire range of movement. """ def __init__(self, motor_id, invert='False'): """Create an instance of the auto stepper motor. Must provide a motor_id parameter with the motor ID from the motor HAT (sent as a string). Can optionally provide: - invert: Set to True to reverse the forward/backward direction. Set this if the motor spins the wrong way when going forward/backward. """ # Create stepper motor driver. self._mh = Adafruit_MotorHAT() self._stepper = self._mh.getStepper(600, int(motor_id)) self._forward = Adafruit_MotorHAT.FORWARD self._backward = Adafruit_MotorHAT.BACKWARD if self.parse_bool(invert): # Invert forward/backward directions self._forward, self._backward = self._backward, self._forward # Rotate forward 600 times to get back into a known 0/home state. for i in range(600): self._stepper.oneStep(self._backward, Adafruit_MotorHAT.DOUBLE) self._steps = 0 def set_value(self, value): """Set the value of the gauge. Must pass in a string with a floating point value in the range 0-100. The dial will be moved to the specified percent location along its range of movement (about 315 degrees). """ # Convert the value to a float percentage. value = float(value) if value < 0.0 or value > 100.0: raise RuntimeError('Value must be in range of 0-100!') # Figure out how many forward or backward steps need to occur to move # to the specified position. new_steps = value/100.0*600.0 delta = int(new_steps - self._steps) direction = self._backward if delta < 0 else self._forward # Move the required number of steps and update the current step location. for i in range(abs(delta)): self._stepper.oneStep(direction, Adafruit_MotorHAT.DOUBLE) self._steps = new_steps
class Motor(): def __init__(self): # create a default object, no changes to I2C address or frequency self.mh = Adafruit_MotorHAT() # create empty threads (these will hold the stepper 1 and 2 threads) self.st = threading.Thread() atexit.register(self.turnOffMotors) self.stepper = self.mh.getStepper(steps_per_rev, 1) # 200 steps/rev, motor port #1 self.stepper.setSpeed(60) # 30 RPM def set_motor_frame(self, frame): if 'speed' in frame: self.rotate(frame['angle'], frame['speed']) else: self.rotate(frame['angle']) def reset(self): self.turnOffMotors() # recommended for auto-disabling motors on shutdown! def turnOffMotors(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) def rotate(self, angle, speed=None): if speed: self.stepper.setSpeed(speed) steps = int((angle / 360) * steps_per_rev) if angle > 0: dir = Adafruit_MotorHAT.FORWARD else: dir = Adafruit_MotorHAT.BACKWARD self.st = threading.Thread(target=stepper_worker, args=( self.stepper, steps, dir, Adafruit_MotorHAT.DOUBLE, )) self.st.start()
def __init__(self, speed): # Create Adafruit Instance hat = Adafruit_MotorHAT() def turnOffMotors(): hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) # Initialize self.speed = speed self.instance = hat.getStepper(48, 1) self.instance.setSpeed(speed) self.running = False # Thread Thread.__init__(self)
class Projector(object): def __init__(self): # create a default object, no changes to I2C address or frequency self.mh = Adafruit_MotorHAT() atexit.register(self.turnOffMotors) # 200 steps/rev, motor port #1 self.projector_motor = self.mh.getStepper(200, 2) # 30 RPM self.projector_motor.setSpeed(50) # recommended for auto-disabling motors on shutdown! def turnOffMotors(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) def lower(self, motor): motor.step(4500, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) def hoist(self, motor): motor.step(4500, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE)
class Blinds(object): def __init__(self): self.distance = 2060 self.adjustment_distance = 10 # create a default object, no changes to I2C address or frequency self.mh = Adafruit_MotorHAT(addr=0x60) # 200 steps/rev, motor port #2 self.blinds = self.mh.getStepper(200, 2) # 30 RPM self.blinds.setSpeed(20) def backward(self): self.blinds.step(self.distance, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) def forward(self): self.blinds.step(self.distance, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) def adjust_forward(self): self.blinds.step(self.adjustment_distance, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) def adjust_backward(self): self.blinds.step(self.adjustment_distance, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE)
class Adafruit_DC_AND_Stepper_HAT: def __init__(self): try: self.setAddress(0x60) except: print("You need to change the HAT Address") sys.quit() self.myStepper = [None, None] def setAddress(self, Address): self.mh = Adafruit_MotorHAT(addr=Address) def setDcMotor(self, motor, direction, speed): if direction == 2: self.mh.getMotor(motor).run(Adafruit_MotorHAT.FORWARD) elif direction == 0: self.mh.getMotor(motor).run(Adafruit_MotorHAT.BACKWARD) else: self.mh.getMotor(motor).run(Adafruit_MotorHAT.RELEASE) myMotor = self.mh.getMotor(motor) speed = speed * 255 myMotor.setSpeed(int(speed)) def setUpStepper(self, steps, portNum): self.myStepper[portNum - 1] = self.mh.getStepper(steps, portNum) def stepStepper(self, steps, dir, type, portNum): if dir == 2: direction = 2 # backward elif dir == 1: direction = 4 # release elif dir == 0: direction = 1 # forward # 3 = break self.myStepper[portNum - 1].step(steps, direction, (type + 1))
class StepperMotor: # Motor hat mh = None # Current motor motor = None def __init__(self, addr, step, port, speed): # Read I2C address self.mh = Adafruit_MotorHAT(addr=addr) # Get stepperMotor self.motor = self.mh.getStepper(port, step) # Set speed self.motor.setSpeed(speed) # Disable motor at exit def TurnOff(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(5).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(6).run(Adafruit_MotorHAT.RELEASE) # Turn stepperMotor def Turn(self, number_of_step, clockwise, type): # Set direction with bool c = Adafruit_MotorHAT.FORWARD if clockwise else Adafruit_MotorHAT.BACKWARD t = Adafruit_MotorHAT.SINGLE if type else Adafruit_MotorHAT.DOUBLE self.motor.step(number_of_step, c, t)
class Turret(): def __init__(self): self.FLYWHEEL_PIN = 24 self.FIRE_PIN = 23 self.STEPS = 5 # self.ammoCounter = AmmoCounter() self.initMotors().initBlaster() # Init stepper motors def initMotors(self): # new Motor HAT self.mh = Adafruit_MotorHAT(addr=0x60) atexit.register(self.disableTurret) #create and set stepper motor objects self.verticalStepper = self.mh.getStepper(200, 2) self.verticalStepper.setSpeed(5) self.horizontalStepper = self.mh.getStepper(200, 1) self.horizontalStepper.setSpeed(5) return self # Init GPIO stuff for blaster def initBlaster(self): #pin for flywheels #always have flywheels on. It will be noisy, but there will be no delay when firing since we dont need to keep toggling the flywheels GPIO.setmode(GPIO.BCM) GPIO.setup(self.FLYWHEEL_PIN, GPIO.OUT) GPIO.output(self.FLYWHEEL_PIN, GPIO.LOW) #pin for firing GPIO.setmode(GPIO.BCM) GPIO.setup(self.FIRE_PIN, GPIO.OUT) GPIO.output(self.FIRE_PIN, GPIO.HIGH) return self # Functions for aiming/angling/rotating blaster # Using threading to be able to control more than 1 motor at the same time def rotateUp(self): print "rotating up!" # rotateUp_Thread = threading.Thread(target = stepperWrapper, args = (self.verticalStepper, self.STEPS, Adafruit_MotorHAT.FORWARD)) # rotateUp_Thread.start() self.verticalStepper.step(self.STEPS, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) return self def rotateDown(self): print "rotating down!" # rotateDown_Thread = threading.Thread(target = stepperWrapper, args = (self.verticalStepper, self.STEPS, Adafruit_MotorHAT.BACKWARD)) # rotateDown_Thread.start self.verticalStepper.step(self.STEPS, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) return self def rotateRight(self): print "rotating right!" # rotateRight_Thread = threading.Thread(target = stepperWrapper, args = (self.horizontalStepper, self.STEPS, Adafruit_MotorHAT.FORWARD)) # rotateRight_Thread.start() self.horizontalStepper.step(self.STEPS, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) return self def rotateLeft(self): print "rotating left!" # rotateLeft_Thread = threading.Thread(target = stepperWrapper, args = (self.horizontalStepper, self.STEPS, Adafruit_MotorHAT.BACKWARD)) # rotateLeft_Thread.start() self.horizontalStepper.step(self.STEPS, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) return self #auto disable all motors and relays on shutdown def disableTurret(self): self.disableStepperMotors() # auto-disable motors on shutdown def disableStepperMotors(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) return self #disable blaster. This includes shutting off relay and clearing GPIO def disableBlaster(self): GPIO.output(self.FIRE_PIN, GPIO.HIGH) GPIO.output(self.FLYWHEEL_PIN, GPIO.HIGH) GPIO.cleanup() return self def shoot(self): print "shooting! from nerfBlasterTurret" GPIO.output(self.FIRE_PIN, GPIO.LOW) time.sleep(.2) GPIO.output(self.FIRE_PIN, GPIO.HIGH) return self
# License along with this library; if not, write to the Free # Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, # MA 02111-1307, USA import smbus, os import time import math import threading from LSM9DS0 import * from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor import datetime bus = smbus.SMBus(1) #Stepper Motors mh = Adafruit_MotorHAT() StepperLeft = mh.getStepper(0, 2) StepperRight = mh.getStepper(0, 1) SleepCounter = 0 Calibrating = True CalibratingCounter = 0 BalancePoint = 0 BalanceValue = 0 Move = 0 Counter = 0 MoveType = Adafruit_MotorHAT.SINGLE RAD_TO_DEG = 57.29578 M_PI = 3.14159265358979323846 G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly AA = 0.40 # Complementary filter constant
class StepperController: def __init__(self, num, logger=None): self._number = num self._ppr = 96 # pulses per revolution for stepper motor self.mh = Adafruit_MotorHAT() # create a default object, no changes to I2C address or frequency self.myStepper = self.mh.getStepper(self._ppr, self._number) #96 steps per revolution. Stepper port num. self.mypwm = Adafruit_PWM_Servo_Driver.PWM(0x60, debug=False) #I2C address 0x60 self.logger = logger or logging.getLogger(__name__) self.speed_pps = 20 #Speed in pulses per second self.steps = 0 self.step_type = 0 #SINGLE = 1 DOUBLE = 2 INTERLEAVE = 3 MICROSTEP = 4 self.motorq = Queue.Queue(5) self.motor_pos = 0 self.motor_pos_new = 0 self.motor_speed = 0 self.motor_half_step = True self.motor_running = False self.inv_dir = True #Constants from motorhat library num -= 1 self.FORWARD = 1 self.BACKWARD = 2 self.BRAKE = 3 self.RELEASE = 4 if (num == 0): self.PWMA = 8 self.AIN2 = 9 self.AIN1 = 10 self.PWMB = 13 self.BIN2 = 12 self.BIN1 = 11 elif (num == 1): self.PWMA = 2 self.AIN2 = 3 self.AIN1 = 4 self.PWMB = 7 self.BIN2 = 6 self.BIN1 = 5 else: self.logger.error("MotorHAT Stepper must be between 1 and 2 inclusive") def write_motorq(self, command_tuple): #Tuple: command, speed, direction, motor.handle self.motorq.put(command_tuple) self.logger.debug("Putting " + str(command_tuple) + " : " + str(self.motorq.qsize()) + " items in queue") # Disable motors/ no force on poles! def turn_off_all(self ): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) def motor_set_speed(self, speed): self.motor_speed = speed self.speed_pps = float(speed)/60*self._ppr self.logger.info("Speed: {} RPM".format(speed)) if self.speed_pps > 95 and self.speed_pps < 193: self.mypwm.setPWMFreq( self.speed_pps/4) # Speed is 4 steps per frequency puls elif self.speed_pps < 96: self.mypwm.setPWMFreq(1600) self.myStepper.setSpeed(speed) else: self.logger.error("Speed {} rpm is to high or otherwise wrong".format(speed)) return def motor_set_step_type(self, half_step): self.motor_half_step = half_step if half_step: self.step_type = 3 else: self.step_type = 2 return def start_motor (self, position=None): if self.step_type != 2 and self.step_type != 3: self.motor_set_step_type(self.motor_half_step) if not position == None: self.motor_pos_new = position direction = self.FORWARD if self.motor_pos_new > self.motor_pos else self.BACKWARD #check direction (forward is True) if self.speed_pps > 95 and self.speed_pps < 193: #High speed stepping self.motorq.put(["fast", position, direction]) self.logger.debug("Putting " + str(["fast", position, direction]) + " : " + str(self.motorq.qsize()) + " items in queue") elif self.speed_pps < 96: self.motorq.put(["slow", position, direction]) self.logger.debug("speed_pps: {}".format(self.speed_pps)) self.logger.debug("Putting " + str(["slow", position, direction]) + " : " + str(self.motorq.qsize()) + " items in queue") else: self.logger.error("Speed {} rpm is to high or otherwise wrong".format(self.motor_speed)) return def release_motor (self): self.mh.setPin(self.AIN2, 0) self.mh.setPin(self.BIN1, 0) self.mh.setPin(self.AIN1, 0) self.mh.setPin(self.BIN2, 0) '''if self._number == 2: self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) else : self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE)''' def stop_motor (self): self.motorq.put(["stop", 0]) self.myStepper.stop_stepper = True return def async_stop(self): self.myStepper.stop_stepper = True self.motorq.put(["exit", 0]) return def calc_pos(self, direction, start_time, run_type, step_count): if run_type == "slow": #if step_count != None: if self.motor_pos_new > self.motor_pos: self.motor_pos = self.motor_pos + step_count else: self.motor_pos = self.motor_pos - step_count elif run_type == "fast": if self.motor_pos_new > self.motor_pos: self.motor_pos =self.motor_pos + int((time.time() - start_time)*self.speed_pps * (2 if self.motor_half_step else 1)) else: self.motor_pos =self.motor_pos - int((time.time() - start_time)*self.speed_pps * (2 if self.motor_half_step else 1)) def async_motor (self): steps = 0 start_time = 0 run_time = 0 step_count = 0 running_type = "fast" while True: if not self.motorq.empty(): #Get items from Queue item = self.motorq.get() command = item[0] value = item[1] if len(item) == 3: motor_dir = item[2] elif len(item) > 3: motor_dir = item[2] motor_handle = item[3] self.logger.info("Getting " + str(item) + " : " + str(self.motorq.qsize()) + " items in queue") #direction = self.motor_pos_new > self.motor_pos #True = going out. steps = int(abs(self.motor_pos_new - self.motor_pos)) #Add steps if going out if command == "exit": self.turn_off_all() break elif command == "fast": running_type = command try: if self.motor_half_step: #t= steps/pulses per second run_time = (float(abs(steps))/float(self.speed_pps))/2 else: run_time = (float(abs(steps))/float(self.speed_pps)) if run_time > 0.1: if self.step_type == 2: if (motor_dir != self.inv_dir): self.mypwm.setPWM(self.AIN2, 0, 2048) self.mypwm.setPWM(self.BIN1, 1024, 3072) self.mypwm.setPWM(self.AIN1, 2048, 4095) self.mypwm.setPWM(self.BIN2, 3072, 512) else: self.mypwm.setPWM(self.AIN2, 3072, 512) self.mypwm.setPWM(self.BIN1, 2048, 4095) self.mypwm.setPWM(self.AIN1, 1024, 3072) self.mypwm.setPWM(self.BIN2, 0, 2048) elif self.step_type == 3: if (motor_dir != self.inv_dir): self.mypwm.setPWM(self.AIN2, 0, 1536) self.mypwm.setPWM(self.BIN1, 1024, 2560) self.mypwm.setPWM(self.AIN1, 2048, 3584) self.mypwm.setPWM(self.BIN2, 3072, 512) else: self.mypwm.setPWM(self.AIN2, 3072, 512) self.mypwm.setPWM(self.BIN1, 2048, 3584) self.mypwm.setPWM(self.AIN1, 1024, 2560) self.mypwm.setPWM(self.BIN2, 0, 1536) else: self.logger.error("Other step modes currently not supported") start_time = time.time() self.mypwm.setPWM(self.PWMA, 0, 4095) self.mypwm.setPWM(self.PWMB, 0, 4095) self.motor_running = True self.logger.info("Going to position {}".format(self.motor_pos_new)) else: pass except: self.logger.error("Start fast fault") elif command == "slow": running_type = command try: if self.step_type == 3: #t= steps/pulses per second run_time = (float(abs(steps))/float(self.speed_pps))/2 else: run_time = (float(abs(steps))/float(self.speed_pps)) start_time = time.time() self.motor_running = True self.logger.info("Going to position {}".format(self.motor_pos_new)) step_count = self.myStepper.step(steps, (motor_dir != self.inv_dir), self.step_type) self.release_motor() except: self.logger.exception("Start slow fault") elif command == "DCMotor": motor_handle.setSpeed(value) motor_handle.run(motor_dir) elif command == "stop" and self.motor_running: self.release_motor() self.motor_running = False self.calc_pos(motor_dir, start_time, running_type, step_count) run_time = 0 self.logger.info("Reached position {}, {}".format(self.motor_pos, motor_dir)) else: pass # Catch the time to stop the execution of fast moving motor. if self.motor_running and time.time() - start_time >= run_time: stop_time = time.time() self.logger.debug("Running time: {}, expected time: {}".format(stop_time - start_time, run_time)) run_time = 0 self.motor_running = False if running_type == "fast": self.mypwm.setPWM(self.PWMA, 0, 0) self.mypwm.setPWM(self.PWMB, 0, 0) self.calc_pos(motor_dir, start_time, running_type, step_count) elif running_type == "slow": self.calc_pos(motor_dir, start_time, running_type, step_count) self.logger.info("Reached position: {}, {}".format(self.motor_pos, motor_dir)) time.sleep(0.05) return
# create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper = mh.getStepper(200, 2) # 200 steps port 2 myStepper.setSpeed(255) # 150 RPM #while (True): # print("Single coil steps") #myStepper.step(1020, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) # myStepper.step(840, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) # print("Double coil steps") #myStepper.step(1020, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) #myStepper.step(1020, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) # print("Interleaved coil steps") # 1015 full rotation myStepper.step(2030, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE)
def main(): #### Setup stepper motor #### # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr=0x60) # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) mh = Adafruit_MotorHAT(addr=0x60) myStepper = mh.getStepper(200, 1) # 200 steps/rev, motor port #1 myStepper.setSpeed(30) # 30 RPM #### setup camera #### """Face detection camera inference example.""" parser = argparse.ArgumentParser() parser.add_argument( '--num_frames', '-n', type=int, dest='num_frames', default=-1, help='Sets the number of frames to run for, otherwise runs forever.') args = parser.parse_args() with PiCamera() as camera: # Forced sensor mode, 1640x1232, full FoV. See: # https://picamera.readthedocs.io/en/release-1.13/fov.html#sensor-modes # This is the resolution inference run on. camera.sensor_mode = 4 # Scaled and cropped resolution. If different from sensor mode implied # resolution, inference results must be adjusted accordingly. This is # true in particular when camera.start_recording is used to record an # encoded h264 video stream as the Pi encoder can't encode all native # sensor resolutions, or a standard one like 1080p may be desired. camera.resolution = (1640, 1232) # Start the camera stream. camera.framerate = 30 # Tempolary disable camera preview so that I can see the log on Terminal camera.start_preview() # Annotator renders in software so use a smaller size and scale results # for increased performace. annotator = Annotator(camera, dimensions=(320, 240)) scale_x = 320 / 1640 scale_y = 240 / 1232 # Incoming boxes are of the form (x, y, width, height). Scale and # transform to the form (x1, y1, x2, y2). def transform(bounding_box): x, y, width, height = bounding_box return (scale_x * x, scale_y * y, scale_x * (x + width), scale_y * (y + height)) with CameraInference(face_detection.model()) as inference: for i, result in enumerate(inference.run()): if i == args.num_frames: break faces = face_detection.get_faces(result) annotator.clear() for face in faces: annotator.bounding_box(transform(face.bounding_box), fill=0) # Print the (x, y) location of face print('X = %d, Y = %d' % (face.bounding_box[0], face.bounding_box[1])) # Move stepper motor if face.bounding_box[0] > 1640 / 2 and abs( face.bounding_box[0] - 1640 / 2) > 200: print("Double coil step - right") myStepper.step(10, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) elif face.bounding_box[0] < 1640 / 2 and abs( face.bounding_box[0] - 1640 / 2) > 300: print("Double coil step - left") myStepper.step(10, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) annotator.update() # print('Iteration #%d: num_faces=%d' % (i, len(faces))) camera.stop_preview()
# create empty threads (these will hold the stepper 1 and 2 threads) st1 = threading.Thread() st2 = threading.Thread() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper1 = mh.getStepper(200, 1) # 200 steps/rev, motor port #1 myStepper2 = mh.getStepper(200, 2) # 200 steps/rev, motor port #1 myStepper1.setSpeed(60) # 30 RPM myStepper2.setSpeed(60) # 30 RPM stepstyles = [Adafruit_MotorHAT.SINGLE, Adafruit_MotorHAT.DOUBLE, Adafruit_MotorHAT.INTERLEAVE, Adafruit_MotorHAT.MICROSTEP] def stepper_worker(stepper, numsteps, direction, style): #print("Steppin!") stepper.step(numsteps, direction, style) #print("Done") while (True): if not st1.isAlive(): randomdir = random.randint(0, 1)
## Stepper Motors from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr = 0x60) # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor[1].run(Adafruit_MotorHAT.RELEASE) mh.getMotor[2].run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) AzStepper = mh.getStepper(200, int(settings['Motors']['azmotnum'])) # 200 steps/rev, motor port #1 ElStepper = mh.getStepper(200, int(settings['Motors']['elmotnum'])) # 200 steps/rev, motor port #2 AzStepper.setSpeed(30) ElStepper.setSpeed(30) LastServerReadTime = 0 LastCycleTime = 0 while utilfcn.str2bool(settings['Program']['run']): # if settings have been updated, reload them if(datetime.datetime.fromtimestamp(os.stat(settings_filename).st_mtime) > IniCheckTime): print('Ini updated, reloading.') settings = utilfcn.ini2dict(settings_filename);
stepperThreads = [threading.Thread(), threading.Thread(), threading.Thread()] # recommended for auto-disabling motors on shutdown! def turnOffMotors(): tophat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) tophat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) tophat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) tophat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) bottomhat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) bottomhat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) bottomhat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) bottomhat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper1 = bottomhat.getStepper(200, 1) # 200 steps/rev, motor port #1 myStepper2 = bottomhat.getStepper(200, 2) # 200 steps/rev, motor port #2 myStepper3 = tophat.getStepper(200, 1) # 200 steps/rev, motor port #1 myStepper1.setSpeed(60) # 60 RPM myStepper2.setSpeed(30) # 30 RPM myStepper3.setSpeed(15) # 15 RPM # get a DC motor! myMotor = tophat.getMotor(3) # set the speed to start, from 0 (off) to 255 (max speed) myMotor.setSpeed(150) # turn on motor myMotor.run(Adafruit_MotorHAT.FORWARD);
import time import sys sys.path.insert(0, "../lib/Adafruit_MotorHAT_mod") from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor import Adafruit_PWM_Servo_Driver mh = Adafruit_MotorHAT() # create a default object, no changes to I2C address or frequency myStepper = mh.getStepper(96, 2) print myStepper
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor import time mh = Adafruit_MotorHAT(addr=0x60) # Motor 1 = Left, Motor 2 = Right motor1 = mh.getMotor(1) motor2 = mh.getMotor(2) motor3 = mh.getStepper(100,2) # set the speed to start, from 0 (off) to 255 (max speed) motor1.setSpeed(150) motor2.setSpeed(150) motor3.setSpeed(30) def leftForward(): motor1.run(Adafruit_MotorHAT.FORWARD) return def rightForward(): motor2.run(Adafruit_MotorHAT.FORWARD) return def leftBackward(): motor1.run(Adafruit_MotorHAT.BACKWARD) return def rightBackward():
#!/usr/bin/python from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperM$ import time import atexit #step 1: mh = Adafruit_MotorHAT(addr = 0x60) Axis1 = mh.getStepper(200,1) Axis2 = mh.getStepper(200,2) #make a wrapper for threading def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) def stepper_worker(stepper, numsteps, direction, style) #print("onestep") stepper.step(numsteps, direction, style) #print("Done") H = threading.Thread(target=stepper_worker, args=(axis1, numsteps, direction, step_style)
class StepMotor: #init gpio and rotate motor to initial position (0 degrees) def __init__( self ): # create a default object, no changes to I2C address or frequency self.mh = Adafruit_MotorHAT( addr=0x60 ) # 200 steps/rev, motor port #1 self.stepper = self.mh.getStepper( 200, 1 ) # 30 RPM self.stepper.setSpeed(30) # Start with the motor off self.turnOff(); #current motor position in degrees [0,360] self.motorPosDeg = 0 # Initalise the starting position self.initPos() # Register the turn off function atexit.register( self.turnOff ) #rotate motor in clockwise direction def rotateR( self, noSteps, speed ): # Set the speed self.stepper.setSpeed( speed ) # Start stepping: SINGLE, DOUBLE, INTERLEAVE, MICROSTEP self.stepper.step( noSteps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP ) # Turn off the motor self.turnOff() # Get the motor position self.motorPosDeg -= noSteps * 1.8 # If the position is less than 0 then add the position to 360 # (pos is going to be a negative which is why we add the position to 360) if ( self.motorPosDeg < 0 ): self.motorPosDeg = 360 + self.motorPosDeg # Store the position just in case we loose power self.storePos() #rotate motor in counterclockwise direction def rotateL( self, noSteps, speed ): # Set the speed self.stepper.setSpeed( speed ) # Start stepping: SINGLE, DOUBLE, INTERLEAVE, MICROSTEP self.stepper.step( noSteps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.MICROSTEP ) # Turn off the motor self.turnOff() # Get the motor position self.motorPosDeg += noSteps * 1.8 # If the position is greater than 3060 then reset the rotation if ( self.motorPosDeg >= 360 ): self.motorPosDeg = self.motorPosDeg - 360 # Store the position just in case we loose power self.storePos() #rotate motor to specific position def rotateToAngle( self, desiredAngle, speed ): s = 1 # print 'DESIRED ANGLE: %s\n' %str( desiredAngle ) # print 'CURRENT POS: %s\n' %str( self.motorPosDeg ) if desiredAngle == self.motorPosDeg: return deltaAngle = abs( self.motorPosDeg - desiredAngle ) if ( desiredAngle > self.motorPosDeg ): if ( deltaAngle >= 180 ): self.rotateR( int(( 360 - deltaAngle ) / 1.8 ), s ) else: self.rotateL( int( deltaAngle / 1.8 ), s ) else: if ( deltaAngle >= 180 ): self.rotateL( int(( 360 - deltaAngle ) / 1.8 ), s ) else: self.rotateR( int( deltaAngle / 1.8 ), s ) # Go to initial position defined by optical sensor def initPos( self ): data = None if ( os.path.isfile( 'objs.pickle' )): with open('objs.pickle') as f: data = pickle.load( f ) if data : self.motorPosDeg = data[0] else : self.motorPosDeg = 0 print 'START POS: %s\n' %str( self.motorPosDeg ) # Rotate the turntable to 0 so it has "reset" self.rotateToAngle( 0, 0 ) def storePos( self ): # print 'CURRENT POS: %s\n' %str( self.motorPosDeg ) # Saving the objects: with open('objs.pickle', 'w') as f: pickle.dump([ self.motorPosDeg ], f ) #turn coils off # recommended for auto-disabling motors on shutdown! def turnOff( self ): # Turn off the motor self.mh.getMotor(1).run( Adafruit_MotorHAT.RELEASE ) self.mh.getMotor(2).run( Adafruit_MotorHAT.RELEASE ) self.mh.getMotor(3).run( Adafruit_MotorHAT.RELEASE ) self.mh.getMotor(4).run( Adafruit_MotorHAT.RELEASE ) def cleanup( self ): # Turn off the motors self.turnOff()
class StepperActuator(Actuator): ''' Class for controlling any and all linear actuators in the design Since we are using the same actuator for all actuation, this should be the only class to implement. Granted, we'll use each actuator differently, but all will be instances of this StepperActuator class (barring changes) The class must override the five private methods from Actuator - the function of each is described in Actuator. ''' class StepType(enum.IntEnum): if onPI: single = Adafruit_MotorHAT.SINGLE double = Adafruit_MotorHAT.DOUBLE micro = Adafruit_MotorHAT.MICROSTEP interleave = Adafruit_MotorHAT.INTERLEAVE else: single = 0 double = 1 micro = 2 interleave = 3 logger = logging.getLogger('cookiebot.Actuator.StepperActuator') def __init__(self, identity='', peak_rpm=30, dist_per_step=1.0, max_dist=1000000000, addr=0x60, steps_per_rev=200, stepper_num=1, step_type=StepType.double, reversed=False, zero_pins={'start': 4, 'end': 4}): ''' Constructor Takes the arguments above (and any other startup information needed, like pins, addresses, etc - add them as keyword arguments to the constructor) and prepares an actuator for use. Connecting to hats and zeroing starting position goes here ''' # superclass constructor run_interval = 1.0 / (peak_rpm * 200.0 / 60.0) super(StepperActuator, self).__init__( identity=identity, run_interval=run_interval) self.step_style = step_type self.step_pos = 0 self.step_size = dist_per_step self.max_steps = int(max_dist / self.step_size) self.zero_pins = zero_pins if onPI: self.hat = Adafruit_MotorHAT(addr=addr) self.stepper = self.hat.getStepper(steps_per_rev, stepper_num) self.motors = [1, 2] if stepper_num == 1 else [3, 4] if reversed: self.forward = Adafruit_MotorHAT.BACKWARD self.backward = Adafruit_MotorHAT.FORWARD else: self.forward = Adafruit_MotorHAT.FORWARD self.backward = Adafruit_MotorHAT.BACKWARD #for pin in self.zero_pins.itervalues(): # GPIO.setup(pin, GPIO.IN, pull_up_down=GPIO.PUD_UP) else: self.hat = None self.stepper = None self.motors = [] def set_rpm(self, new_rpm): """Set a new rpm value for this StepperActuator""" new_interval = 1.0 / (new_rpm * 200.0 / 60.0) self._timer.interval = new_interval def go_to_zero(self): pin_to_listen = self.zero_pins['start'] # do stuff here - how does GPIO work? if onPI: #while GPIO.input(pin_to_listen) == GPIO.HIGH: # time.sleep(0.01) # self.stepper.oneStep( # self.backward, self.step_style.value) pass self.step_pos = 0 def kill(self): super(StepperActuator, self).kill() if onPI: for m in self.motors: self.hat.getMotor(m).run(Adafruit_MotorHAT.RELEASE) @property def real_pos(self): return self.step_pos * self.step_size def _check_bounds(self): """TBD""" if onPI: return True #return all([GPIO.input(p) == GPIO.HIGH for p in self.zero_pins.values()]) else: return True def _validate_task(self, task): '''Check that task is an iterable containing only -1, 0 or 1''' try: itertask = iter(task) except TypeError: return False else: return set(itertask) <= set((-1, 0, 1)) def _task_is_complete(self): return not self._task def _execute_task(self): step, self._task = self._task[0], self._task[1:] # aka generalized pop self.step_pos += step if onPI: if step == -1: # step back oneStep self.stepper.oneStep(self.backward, self.step_style.value) elif step == 1: # step forward oneStep self.stepper.oneStep(self.forward, self.step_style.value)
from time import sleep # turn off all motors def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) print("Motors released") # motor parameters mh = Adafruit_MotorHAT(addr=0x60) atexit.register(turnOffMotors) myStepper = mh.getStepper(200, 1) myStepper.setSpeed(200) def led_blue(): GPIO.setmode(GPIO.BOARD) GPIO.setup(11, GPIO.OUT) # RGB red GPIO.output(11, GPIO.LOW) GPIO.setup(13, GPIO.OUT) # RGB green GPIO.output(13, GPIO.LOW) GPIO.setup(15, GPIO.OUT) # RGB blue GPIO.output(15, GPIO.LOW) chan_list = (11, 13, 15) GPIO.output(chan_list, (GPIO.LOW, GPIO.LOW, GPIO.HIGH)) sleep(6)
from Adafruit_MotorHAT import Adafruit_MotorHAT import time HAT = Adafruit_MotorHAT stepper_hat = Adafruit_MotorHAT() stepper = stepper_hat.getStepper(200, 1) # 200 steps/rev, port 1 (M1, M2) try: while True: speed = input("Enter stepper speed (rpm) ") stepper.setSpeed(speed) steps_forward = input("Steps forward ") stepper.step(steps_forward, HAT.FORWARD, HAT.SINGLE) steps_forward = input("Steps reverse ") stepper.step(steps_forward, HAT.BACKWARD, HAT.SINGLE) finally: print("cleaning up") stepper_hat.getMotor(1).run(HAT.RELEASE)
import time import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr=0x70) # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper = mh.getStepper(4076, 1) # 200 steps/rev, motor port #1 myStepper.setSpeed(15) # 30 RPM print("Single coil steps") myStepper.step(7*255, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) #myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) #print("Double coil steps") #myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) #myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) #print("Interleaved coil steps") #myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) #myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE)
#!/usr/bin/env python from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_StepperMotor import RPi.GPIO as GPIO #import cv2 import time # Stepper setup mh = Adafruit_MotorHAT(addr = 0x60) stepper = mh.getStepper(200, 1) stepper.setSpeed(30) # Set up camera #vc = cv2.VideoCapture(0) # Set up ML # Turn on Adafruit MotorHAT (BJT wired to pin 21) GPIO.setmode(GPIO.BCM) GPIO.setup(21, GPIO.OUT) GPIO.output(21, True) while True: stepper.step(200, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) time.sleep(1) print "I took a photo" # Take image and process #retval, image = vc.read() #if (retval) #proc ML # Turn off Adafruit MotorHAT
class Stepper(threading.Thread): def __init__(self,name,stepper_address ,port,io_address,pin_low_limit_sns,pin_high_limit_sns): super(Stepper, self).__init__() self.setDaemon(True) self.running = False # Create stepper objects self.name = name self.stepper_address= stepper_address self.port= port # motorhat uses port 1 and 2 self.io_address=io_address self.pin_high_limit_sns = pin_high_limit_sns self.pin_low_limit_sns = pin_low_limit_sns #defaults self.isEnabled = True self.number_of_steps = 0 self.direction = Adafruit_MotorHAT.BRAKE self.style = Adafruit_MotorHAT.INTERLEAVE #create MotorHat object self.motor_hat = Adafruit_MotorHAT(self.stepper_address) #create stepper object self.stepper = self.motor_hat.getStepper(200,self.port) self.stepper.setSpeed(60) #create IO object self.expander=PCA9555(self.io_address) # flow control def shutdown(self): self.running=False def enable(self): self.isEnabled=True def disable(self): self.isEnabled=False def run(self): #started by thread.start() method initial position # print("{} started!".format(self.getName())) # "Thread-x started!" self.running=True while self.running: if self.isEnabled: if not self.expander.digitalRead(self.pin_high_limit_sns): self.oneStep_up() else: sleep(.05) #make sure it hits the switch and doesn't bounce self.stop() self.running=False #self.running-False exits thread # Pretend to work for a second # print("{} finished!".format(self.getName())) # "Thread-x finished!" #stepper commands def up(self): self.stepper.step(4,Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) def down(self): self.stepper.step(4,Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) def stop(self): self.stepper.step(0,Adafruit_MotorHAT.BRAKE,Adafruit_MotorHAT.INTERLEAVE) def release(self): self.stepper.step(0,Adafruit_MotorHAT.RELEASE,Adafruit_MotorHAT.INTERLEAVE) def oneStep_up(self): self.stepper.oneStep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) def oneStep_down(self): self.stepper.oneStep(Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) #generic API def step(self,number_of_steps, direction, style): self.number_of_steps = number_of_steps self.direction = direction self.style = style self.stepper.step(self.number_of_steps, self.direction,self.style) def oneStep(self,direction, style): self.direction = direction self.style = style self.stepper.oneStep(self.direction, self.style) def e_stop(self): #coil 1 and 2, 3 and 4 #TODO: rewrite use 1=true,etc if self.port == 1: self.motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) elif self.port == 2: self.motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) else: self.motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
if desiredangle != prevangle: desiredangle = prevangle - desiredangle if desiredangle == 0: zero_angleY() elif desiredangle > 0: angledownY(desiredangle) elif desiredangle < 0: angleupY(-1*desiredangle) if __name__ == '__main__': # create a default object, with changes to I2C address but not frequency mh = Adafruit_MotorHAT(addr=0x61) # top hat motors atexit.register(turnOffMotors) myStepper = mh.getStepper(200, 1) # 200 steps/rev, upper motor port #1 myStepper2 = mh.getStepper(200, 2) # 200 steps/rev, upper motor port #2 myStepper.setSpeed(30) # 30 RPM myStepper2.setSpeed(30) angleX=float(raw_input("X Angle: ")) angleY=float(raw_input("Y Angle: ")) stepsX = int(angleX/1.8) stepsY = int(angleY/1.8) print "X Steps:",stepsX print "Y Steps:",stepsY print("Double coil steps")
import time import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper = mh.getStepper(200, 2) # 200 steps/rev, motor port #1 myStepper.setSpeed(10) # 10 RPM while (True): print("Single coil steps") myStepper.step(200, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) myStepper.step(200, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) print("Double coil steps") myStepper.step(200, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) myStepper.step(200, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) print("Interleaved coil steps") myStepper.step(200, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) myStepper.step(200, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE)
import time import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper = mh.getStepper(200, 2) # 200 steps/rev, motor port #1 myStepper.setSpeed(30) # 30 RPM while (True): print("Single coil steps") myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) print("Double coil steps") myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) print("Interleaved coil steps") myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE)
class Hardware: motionDetectedBool = False def __init__(self): logging.debug('Loading Config....') self.loadConfig() logging.debug('Hardware init...') self.initializeHardware() logging.debug('Hardware ready.') def loadConfig(self): parser = SafeConfigParser() parser.read('/opt/Catdoor/core/config.ini') self.pin_irsensor_in = parser.getint('pin_settings', 'irsensor_in') self.pin_irsensor2_in = parser.getint('pin_settings', 'irsensor2_in') self.pin_buzzer_out = parser.getint('pin_settings', 'buzzer_out') self.motor_delay = parser.getfloat('motor_settings', 'delay') self.motor_distance = parser.getint('motor_settings', 'distance') def initializeHardware(self): self.mh = Adafruit_MotorHAT() self.myStepper = self.mh.getStepper(200,1) self.myStepper.setSpeed(330) GPIO.setwarnings(False) GPIO.cleanup() GPIO.setmode(GPIO.BCM) GPIO.setup(self.pin_irsensor_in, GPIO.IN) GPIO.setup(self.pin_irsensor2_in, GPIO.IN) GPIO.setup(self.pin_buzzer_out, GPIO.OUT) GPIO.output(self.pin_buzzer_out, GPIO.LOW) GPIO.setwarnings(True) def openDoor(self): steps = self.motor_distance self.myStepper.step(steps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) def closeDoor(self): self.doShortBeeps(5) steps = self.motor_distance self.myStepper.step(steps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) def doShortBeeps(self, num): for i in range(0, num): self.beep() def beep(self): GPIO.output(self.pin_buzzer_out, GPIO.HIGH) time.sleep(0.1) GPIO.output(self.pin_buzzer_out, GPIO.LOW) time.sleep(0.05) def isMotionDetected(self): if self.motionDetectedBool: self.motionDetectedBool = False return True else: return False def resetMotionDetected(self): self.motionDetectedBool = False def activateWatchdog(self): self.workerThread = WatchdogThread(self) self.workerThread.start() def deactivateWatchdog(self): self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.workerThread.exit()
import time import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper = mh.getStepper(50, 1) # 200 steps/rev, motor port #1 myStepper.setSpeed(150) # 30 RPM #myStepper1 = mh.getStepper(200, 2) # 200 steps/rev, motor port #1 #myStepper1.setSpeed(30) # 30 RPM #while (True): #print("Single coil steps") def main(): #myStepper.step(1000, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) myStepper.step(1500, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) #print("Double coil steps") #myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) #myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE)
class Winder(object): # Constants for this stepper motor. These might need to be moved later. motor_port = 1 # Port that stepper motor is connected to. steps_per_rev = 200 # Number of steps/revolution for stepper motor. over_turn = 0.10 # Amount of overturn per revolution. turns_between_pause = 5 # Number of turns before pause and/or reverse direction. def __init__(self, turns_per_day, rpm, turn_type): self.turns_per_day = turns_per_day self.rpm = rpm self.turn_type = turn_type self.rot_count = 0 self.pause_interval() self.steps_per_turn() def turn_off_motors(self): self.motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) def create_motor_hat(self): self.motor_hat = Adafruit_MotorHAT() def create_stepper(self): self.my_stepper = self.motor_hat.getStepper(self.steps_per_rev, self.motor_port) self.my_stepper.setSpeed(self.rpm) def pause_interval(self): self.pause = (60 * 60 * 24) / (self.turns_per_day / self.turns_between_pause) def steps_per_turn(self): self.total_steps = int(self.steps_per_rev * (1 + self.over_turn)) def rotate_watch(self): self.create_motor_hat() self.create_stepper() i = 0 while (i < self.turns_between_pause): if (self.turn_type == "CW"): self.my_stepper.step(self.total_steps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) elif (self.turn_type == "CCW"): self.my_stepper.step(self.total_steps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.MICROSTEP) elif (self.turn_type == "ALT" and self.rot_count % 2 == 0): self.my_stepper.step(self.total_steps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) elif (self.turn_type == "ALT" and self.rot_count % 2 == 1): self.my_stepper.step(self.total_steps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.MICROSTEP) else: print("Bad config.") self.rot_count = self.rot_count + 1 i = i + 1 self.turn_off_motors() # Needed so that motor hat can be killed outside of class. def get_motor_hat(self): return self.motor_hat def get_turn_count(self): return self.rot_count def get_pause_interval(self): return self.pause
class Turret(object): """ Class used for turret control. """ def __init__(self, friendly_mode=True): self.friendly_mode = friendly_mode # create a default object, no changes to I2C address or frequency self.mh = Adafruit_MotorHAT() atexit.register(self.__turn_off_motors) # Stepper motor 1 self.sm_x = self.mh.getStepper(200, 1) # 200 steps/rev, motor port #1 self.sm_x.setSpeed(5) # 5 RPM self.current_x_steps = 0 # Stepper motor 2 self.sm_y = self.mh.getStepper(200, 2) # 200 steps/rev, motor port #2 self.sm_y.setSpeed(5) # 5 RPM self.current_y_steps = 0 # Relay GPIO.setmode(GPIO.BCM) GPIO.setup(RELAY_PIN, GPIO.OUT) GPIO.output(RELAY_PIN, GPIO.LOW) def calibrate(self): """ Waits for input to calibrate the turret's axis :return: """ print "Please calibrate the tilt of the gun so that it is level. Commands: (w) moves up, " \ "(s) moves down. Press (enter) to finish.\n" self.__calibrate_y_axis() print "Please calibrate the yaw of the gun so that it aligns with the camera. Commands: (a) moves left, " \ "(d) moves right. Press (enter) to finish.\n" self.__calibrate_x_axis() print "Calibration finished." def __calibrate_x_axis(self): """ Waits for input to calibrate the x axis :return: """ with raw_mode(sys.stdin): try: while True: ch = sys.stdin.read(1) if not ch: break elif ch == "a": if MOTOR_X_REVERSED: Turret.move_backward(self.sm_x, 5) else: Turret.move_forward(self.sm_x, 5) elif ch == "d": if MOTOR_X_REVERSED: Turret.move_forward(self.sm_x, 5) else: Turret.move_backward(self.sm_x, 5) elif ch == "\n": break except (KeyboardInterrupt, EOFError): print "Error: Unable to calibrate turret. Exiting..." sys.exit(1) def __calibrate_y_axis(self): """ Waits for input to calibrate the y axis. :return: """ with raw_mode(sys.stdin): try: while True: ch = sys.stdin.read(1) if not ch: break if ch == "w": if MOTOR_Y_REVERSED: Turret.move_forward(self.sm_y, 5) else: Turret.move_backward(self.sm_y, 5) elif ch == "s": if MOTOR_Y_REVERSED: Turret.move_backward(self.sm_y, 5) else: Turret.move_forward(self.sm_y, 5) elif ch == "\n": break except (KeyboardInterrupt, EOFError): print "Error: Unable to calibrate turret. Exiting..." sys.exit(1) def motion_detection(self, show_video=False): """ Uses the camera to move the turret. OpenCV ust be configured to use this. :return: """ VideoUtils.find_motion(self.__move_axis, show_video=show_video) def __move_axis(self, contour, frame): (v_h, v_w) = frame.shape[:2] (x, y, w, h) = cv2.boundingRect(contour) # find height target_steps_x = (2 * MAX_STEPS_X * (x + w / 2) / v_w) - MAX_STEPS_X target_steps_y = (2 * MAX_STEPS_Y * (y + h / 2) / v_h) - MAX_STEPS_Y print "x: %s, y: %s" % (str(target_steps_x), str(target_steps_y)) print "current x: %s, current y: %s" % (str( self.current_x_steps), str(self.current_y_steps)) t_x = threading.Thread() t_y = threading.Thread() t_fire = threading.Thread() # move x if (target_steps_x - self.current_x_steps) > 0: self.current_x_steps += 1 if MOTOR_X_REVERSED: t_x = threading.Thread(target=Turret.move_forward, args=( self.sm_x, 2, )) else: t_x = threading.Thread(target=Turret.move_backward, args=( self.sm_x, 2, )) elif (target_steps_x - self.current_x_steps) < 0: self.current_x_steps -= 1 if MOTOR_X_REVERSED: t_x = threading.Thread(target=Turret.move_backward, args=( self.sm_x, 2, )) else: t_x = threading.Thread(target=Turret.move_forward, args=( self.sm_x, 2, )) # move y if (target_steps_y - self.current_y_steps) > 0: self.current_y_steps += 1 if MOTOR_Y_REVERSED: t_y = threading.Thread(target=Turret.move_backward, args=( self.sm_y, 2, )) else: t_y = threading.Thread(target=Turret.move_forward, args=( self.sm_y, 2, )) elif (target_steps_y - self.current_y_steps) < 0: self.current_y_steps -= 1 if MOTOR_Y_REVERSED: t_y = threading.Thread(target=Turret.move_forward, args=( self.sm_y, 2, )) else: t_y = threading.Thread(target=Turret.move_backward, args=( self.sm_y, 2, )) # fire if necessary if not self.friendly_mode: if abs(target_steps_y - self.current_y_steps) <= 2 and abs( target_steps_x - self.current_x_steps) <= 2: t_fire = threading.Thread(target=Turret.fire) t_x.start() t_y.start() t_fire.start() t_x.join() t_y.join() t_fire.join() def interactive(self): """ Starts an interactive session. Key presses determine movement. :return: """ Turret.move_forward(self.sm_x, 1) Turret.move_forward(self.sm_y, 1) print 'Commands: Pivot with (a) and (d). Tilt with (w) and (s). Exit with (q)\n' with raw_mode(sys.stdin): try: while True: ch = sys.stdin.read(1) if not ch or ch == "q": break if ch == "w": if MOTOR_Y_REVERSED: Turret.move_forward(self.sm_y, 5) else: Turret.move_backward(self.sm_y, 5) elif ch == "s": if MOTOR_Y_REVERSED: Turret.move_backward(self.sm_y, 5) else: Turret.move_forward(self.sm_y, 5) elif ch == "a": if MOTOR_X_REVERSED: Turret.move_backward(self.sm_x, 5) else: Turret.move_forward(self.sm_x, 5) elif ch == "d": if MOTOR_X_REVERSED: Turret.move_forward(self.sm_x, 5) else: Turret.move_backward(self.sm_x, 5) elif ch == "\n": Turret.fire() except (KeyboardInterrupt, EOFError): pass @staticmethod def fire(): GPIO.output(RELAY_PIN, GPIO.HIGH) time.sleep(1) GPIO.output(RELAY_PIN, GPIO.LOW) @staticmethod def move_forward(motor, steps): """ Moves the stepper motor forward the specified number of steps. :param motor: :param steps: :return: """ motor.step(steps, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) @staticmethod def move_backward(motor, steps): """ Moves the stepper motor backward the specified number of steps :param motor: :param steps: :return: """ motor.step(steps, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) def __turn_off_motors(self): """ Recommended for auto-disabling motors on shutdown! :return: """ self.mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
class ScannerBase(object): """The base of the 3d scanner (controls rotation via stepper motor). Attributes: motor_hat: default adafruit motor hat object stepper: the stepper motor """ def __init__(self, stepper_steps_per_rev=None, stepper_motor_port=None, switch=None): """Return a ScannerBase object :param steps_per_rev: the number of steps per revolution :param motor_port: the motor port # """ # default to 400 steps/rev, motor port #1 if stepper_steps_per_rev is None: stepper_steps_per_rev = 400 if stepper_motor_port is None: stepper_motor_port = 2 if switch is None: switch = scanner_limit_switch.LimitSwitch() # create a default object, no changes to I2C address or frequency self.motor_hat = Adafruit_MotorHAT() self.stepper = self.motor_hat.getStepper(stepper_steps_per_rev, stepper_motor_port) # default to 1 RPM (only used during reset) self.stepper.setSpeed(1) # note the limit switch self.switch = switch atexit.register(self.turn_off_motors) def move_steps(self, num_steps=None): """Moves the stepper motor the specified number of steps :param num_steps: # of steps to move, defaults to 1 """ if num_steps is None: num_steps = 1 if num_steps < 0: num_steps = abs(num_steps) direction = Adafruit_MotorHAT.FORWARD else: direction = Adafruit_MotorHAT.BACKWARD for _ in itertools.repeat(None, num_steps): self.stepper.oneStep(direction, Adafruit_MotorHAT.MICROSTEP) def move_degrees(self, num_deg=None): """Moves the stepper motor by the specified num_deg, as close as step resolution permits. :param num_deg: angle to move in degrees, defaults to 1 degree """ if num_deg is None: num_deg = 1 self.move_steps(int(round(num_deg * self.get_steps_per_deg()))) def get_num_steps_per_rev(self): """Returns the number of micro-steps in a full rotation""" return 400 * 8 # 400 steps/rev * 8 microsteps/step def get_steps_per_deg(self): """Returns the number of steps per degree""" return 1.0 * self.get_num_steps_per_rev() / 360.0 def reset(self): """Resets the base angle.""" # check that the switch is not already pressed # (edge case where a rising edge event won't occur) if not self.switch.is_pressed(): # Move back to home angle, until hitting limit switch. self.switch.setup_event_detect() while not self.switch.check_for_press(): # use DOUBLE mode for more torque self.stepper.step(2, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) self.switch.destroy() # Move forward off the switch for _ in itertools.repeat(None, 12): self.stepper.step(1, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) # check that the switch is not already pressed # (edge case where a rising edge event won't occur) if not self.switch.is_pressed(): # Move back to home angle, until just hitting limit switch self.switch.setup_event_detect() while not self.switch.check_for_press(): # use DOUBLE mode for more torque self.stepper.step(1, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) self.switch.destroy() def turn_off_motors(self): """Turns off stepper motor, recommended for auto-disabling motors on shutdown!""" self.motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) self.motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
mh = Adafruit_MotorHAT() # clean up function for safe termination def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) # register clean up function atexit.register(turnOffMotors) # gain control of stepper and set its speed moon_stepper = mh.getStepper(200, 1) moon_stepper.setSpeed(3) # determine how many steps to move and move motor def update_phase(current_phase, new_phase): if new_phase == current_phase: return steps = new_phase - current_phase steps = steps + 30 if steps < 0 else steps move_the_motor(steps) # get the phase of the current day def get_new_phase(): date = datetime.now()
m1_direction = Adafruit_MotorHAT.FORWARD while (m1_steps_taken != target_steps): doM1StepAndCount(m1_direction) def angleMotor2(target_angle=FinalAngle): global m2_steps_taken target_steps = int( target_angle * STEPS_PER_ROTATION / 360) % STEPS_PER_ROTATION m2_direction = Adafruit_MotorHAT.BACKWARD while (m2_steps_taken != target_steps): doM1StepAndCount(m2_direction) #set stepper motor variables myStepper1 = mh.getStepper(STEPS_PER_ROTATION, 1) myStepper2 = mh.getStepper(STEPS_PER_ROTATION, 2) #set speed in RPM of steppers myStepper1.setSpeed(30) myStepper2.setSpeed(30) # MOTOR ONE distanceRead1 = sensor1.distance * 100 / 2.54 # if its farther than an inch away, move a full inch? # while(distanceRead1 > lengthRodEnd + 1): # myStepper1.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) # print('Distance Sensor 1: ', distanceRead1) # distanceRead1 = sensor1.distance * 100 / 2.54 # Check which direction to move by ThreadType
# Setting the MotorHat I2C address MotorHat = Adafruit_MotorHAT(addr = 0x60) #Turn Off Motor On Exit def turnOffMotors(): MotorHat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) MotorHat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) MotorHat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) MotorHat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) #Creating the Stepper Motor Object PlayerBlueStepper = MotorHat.getStepper(100, 1) PlayerBlueStepper.setSpeed(30) PlayerRedStepper = MotorHat.getStepper(100, 2) PlayerRedStepper.setSpeed(30) #Moving Motor def MoveBlueSharkLeft(): PlayerBlueStepper.step(1, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) print("Blue Shark Left") def MoveBlueSharkRight(): print("Blue Shark right") PlayerBlueStepper.step(1, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) def BlueSharkStop(): turnOffMotors() def RedSharkStop(): turnOffMotors()
mh = Adafruit_MotorHAT() st1 = threading.Thread() st2 = threading.Thread() def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) rotationMotor = mh.getStepper(50, 1) tiltMotor = mh.getStepper(50, 2) def rotateGross(value, center_pos, motor): motor.setSpeed(60) dir = "NONE" if value < (center_pos - 50): dir = Adafruit_MotorHAT.FORWARD elif value >= (center_pos - 50) and value <= (center_pos + 50): return "NONE" else: dir = Adafruit_MotorHAT.BACKWARD return [1, dir, Adafruit_MotorHAT.DOUBLE]
stepper.oneStep(Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) turn_off_motors() def progress_percent(size_list): """ Creates a builder to calculate percentages of the progress of the cocktail creation :param size_list: Takes as parameter the length of the list """ for i in range(size_list * 3): yield (90 / (size_list * 3)) * (i + 1) MY_STEPPER = MOTOR_HAT.getStepper(200, 1) MY_STEPPER.setSpeed(200) atexit.register(turn_off_motors) @shared_task() def make_cocktail(bottles_list): """ Function for the creation of the cocktail: the displacement and the emptying of the solenoid valves :param bottles_list: Takes in parameter the list with the different elements necessary for the creation of the cocktail """ step_tray = 0 start = True
# Software Foundation, Inc., 59 Temple Place - Suite 330, Boston, # MA 02111-1307, USA import smbus, os import time import math import threading from LSM9DS0 import * from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor import datetime bus = smbus.SMBus(1) #Stepper Motors mh = Adafruit_MotorHAT() StepperLeft = mh.getStepper(0, 2) StepperRight = mh.getStepper(0, 1) SleepCounter = 0 Calibrating = True CalibratingCounter = 0 BalancePoint = 0 Move = 0 RAD_TO_DEG = 57.29578 M_PI = 3.14159265358979323846 G_GAIN = 0.070 # [deg/s/LSB] If you change the dps for gyro, you need to update this value accordingly AA = 0.40 # Complementary filter constant
motor 02 in GPIO 21 - 26 you can calibrate the system with buttons''' mh = Adafruit_MotorHAT() # recommended for auto-disabling motors on shutdown! def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) myStepper1 = mh.getStepper(200, 1) # 200 steps/rev, motor port #1 myStepper1.setSpeed(30) # 30 RPM myStepper2 = mh.getStepper(200, 2) # 200 steps/rev, motor port #2 myStepper2.setSpeed(300) # 30 RPM # create empty threads (these will hold the stepper 1 and 2 threads) st1 = threading.Thread() st2 = threading.Thread() stepstyles = [ Adafruit_MotorHAT.SINGLE, Adafruit_MotorHAT.DOUBLE, Adafruit_MotorHAT.INTERLEAVE, Adafruit_MotorHAT.MICROSTEP ] #--------------------------------------------
import atexit # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr = 0x60) # Automatically disable motors on shutdown def turnOffMotors(): mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) atexit.register(turnOffMotors) # 200 steps/rev, motor port #1 motor_x = mh.getStepper(200, 1) # 30 RPM motor_x.setSpeed(30) print("Single coil steps") motor_x.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) motor_x.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) print("Double coil steps") motor_x.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) motor_x.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) print("Interleaved coil steps") motor_x.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) motor_x.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE)