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 __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)
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)
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)
