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 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 Tracking:
def __init__(self):
self.node_name = rospy.get_name()
self.state = 1
self.trig = None
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.leftMotor = self.motorhat.getMotor(1)
self.rightMotor = self.motorhat.getMotor(2)
self.right_pwm = 60
self.left_pwm = 60
self.leftMotor.setSpeed(self.left_pwm)
self.rightMotor.setSpeed(self.right_pwm)
self.subPosition = rospy.Subscriber("/serial_node/odometry",
Float64MultiArray, self.cbPosition)
rospy.on_shutdown(self.custom_shutdown)
rospy.loginfo("[%s] Initialized!" % self.node_name)
def cbPosition(self, msg):
x = msg.data[0]
y = msg.data[1]
theta = msg.data[2]
theta = theta % (2 * pi)
print x, y, theta
# stages: 1) straight line,
# 2) semi-circle
# 3) straight line again.
def custom_shutdown(self):
self.leftMotor.run(4)
self.rightMotor.run(4)
del self.motorhat
class Motors:
def __init__(self, left_id=1, right_id=2, left_trim=0, right_trim=0):
# Initialize motor HAT and left, right motor.
self._mh = Adafruit_MotorHAT(i2c_bus=1)
self._left = self._mh.getMotor(left_id)
self._right = self._mh.getMotor(right_id)
self._left_trim = left_trim
self._right_trim = right_trim
# Start with motors turned off.
self.stop()
# Configure all motors to stop at program exit
atexit.register(self.stop)
def _left_speed(self, speed):
"""Set the speed of the left motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._left_trim
speed = max(0, min(255,
speed)) # Constrain speed to 0-255 after trimming.
self._left.setSpeed(speed)
def _right_speed(self, speed):
"""Set the speed of the right motor, taking into account its trim offset.
"""
assert 0 <= speed <= 255, 'Speed must be a value between 0 to 255 inclusive!'
speed += self._right_trim
speed = max(0, min(255,
speed)) # Constrain speed to 0-255 after trimming.
self._right.setSpeed(speed)
def stop(self):
"""Stop all movement."""
self._left.run(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_MotorHAT.RELEASE)
def TurnOffMotors(self):
# create a default object, no changes to I2C address or frequency
mh = Adafruit_MotorHAT()
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)
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 turnOffMotors(): mh = Adafruit_MotorHAT(addr=0x61) 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)
class Motor_control:
def __init__(self):
self.mtr_hat = Adafruit_MotorHAT()
#initialize motor connections
self.mtr_azimuth = mtr_hat.getStepper(200, 1) # 200 steps/rev, stepper port #1
self.mtr_pitch = mtr_hat.getStepper(200, 2) # 200 steps/rev, stepper port #2
mtr_azimuth.setSpeed(60)
mtr_pitch.setSpeed(60)
# recommended for auto-disabling motors on shutdown!
def turnOffMotors(self):
self.mtr_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
self.mtr_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
self.mtr_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
self.mtr_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
#Function to move Arm stepper motors
def move(self, steps_azimuth, steps_pitch):
if (steps_azimuth > steps_pitch):
for i in range(0, steps_pitch):
mtr_azimuth.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
mtr_pitch.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
for x in range(0, steps_azimuth - steps_pitch):
mtr_azimuth.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
else:
for i in range(0, steps_azimuth):
mtr_azimuth.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
mtr_pitch.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
for x in range(0, steps_pitch - steps_azimuth):
mtr_pitch.onestep(Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE)
class JetBot:
def __init__(self, speed=200):
self.speed = speed
left_motor_channel = 1
right_motor_channel = 2
self.driver = HAT(i2c_bus=1)
self.left_motor = self.driver.getMotor(left_motor_channel)
self.right_motor = self.driver.getMotor(right_motor_channel)
self.left_motor.setSpeed(self.speed)
self.right_motor.setSpeed(self.speed)
def left(self):
self.left_motor.run(HAT.BACKWARD)
self.right_motor.run(HAT.FORWARD)
def right(self):
self.left_motor.run(HAT.FORWARD)
self.right_motor.run(HAT.BACKWARD)
def forward(self):
self.left_motor.run(HAT.FORWARD)
self.right_motor.run(HAT.FORWARD)
def backward(self):
self.left_motor.run(HAT.BACKWARD)
self.right_motor.run(HAT.BACKWARD)
def stop(self):
self.left_motor.run(HAT.RELEASE)
self.right_motor.run(HAT.RELEASE)
def setSpeed(self, speed):
self.speed = speed
self.left_motor.setSpeed(self.speed)
self.right_motor.setSpeed(self.speed)
def turnOffMotors(): # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT() 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)
class RPiCar(Car):
'''
Controls the speed of the car based on desired tangential and rotational speed.
Motors are ramped rather than stepping between speeds
Attributes:
speed_ratio
'''
def __init__(self):
Car.__init__(self)
self._calibration = {}
self._calibration["offset_left"] = 0.2
self._calibration["gain_left"] = 0.8
self._calibration["offset_right"] = 0.2
self._calibration["gain_right"] = 0.8
self._mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(self._turn_off_motors)
self._left_motor = self._mh.getMotor(1)
self._right_motor = self._mh.getMotor(2)
def _turn_off_motors(self):
self._left_motor.run(Adafruit_MotorHAT.RELEASE)
self._right_motor.run(Adafruit_MotorHAT.RELEASE)
def refresh_motor_speed(self, speed, omega):
speed_left = (speed * self.speed_ratio - omega *
(1 - self.speed_ratio))
speed_right = (speed * self.speed_ratio + omega *
(1 - self.speed_ratio))
if speed_left > 0:
speed_left = self._calibration["offset_left"] + speed_left * (
1 - self._calibration["offset_left"])
self._left_motor.setSpeed(int(speed_left * 255))
self._left_motor.run(Adafruit_MotorHAT.FORWARD)
elif speed_left == 0:
self._left_motor.setSpeed(0)
else: # speed_left < 0
speed_left = self._calibration["offset_left"] - speed_left * (
1 - self._calibration["offset_left"])
self._left_motor.setSpeed(int(speed_left * 255))
self._left_motor.run(Adafruit_MotorHAT.BACKWARD)
if speed_right > 0:
speed_right = self._calibration["offset_right"] + speed_right * (
1 - self._calibration["offset_right"])
self._right_motor.setSpeed(int(speed_right * 255))
self._right_motor.run(Adafruit_MotorHAT.FORWARD)
elif speed_right == 0:
self._right_motor.setSpeed(0)
else: # speed_right < 0
speed_right = self._calibration["offset_right"] - speed_right * (
1 - self._calibration["offset_right"])
self._right_motor.setSpeed(int(speed_right * 255))
self._right_motor.run(Adafruit_MotorHAT.BACKWARD)
class Motors:
def __init__(self, brainz=None, verbose=False):
self.verbose = verbose
self.brainz = brainz
self.started = False
self.cycle_time = 0
def __print(self, str):
if self.verbose:
print ( str)
def start(self):
self.started = True
self.mh = Adafruit_MotorHAT(addr=0x60)
self.leftMotor = self.mh.getMotor(1)
self.leftMotor.setSpeed(0)
self.leftMotor.run(Adafruit_MotorHAT.FORWARD)
self.leftMotor.run(Adafruit_MotorHAT.RELEASE)
self.rightMotor = self.mh.getMotor(2)
self.rightMotor.setSpeed(0)
self.rightMotor.run(Adafruit_MotorHAT.FORWARD)
self.rightMotor.run(Adafruit_MotorHAT.RELEASE)
def stop(self):
self.leftMotor.run(Adafruit_MotorHAT.RELEASE)
self.rightMotor.run(Adafruit_MotorHAT.RELEASE)
def tick(self,interval):
if not self.started:
return
self.cycle_time += interval
self.set_speed()
def set_speed(self):
new_speed = 0
self.__print("Speed:" + str(self.brainz.speed))
if self.brainz.low_speed_percent < 1:
new_speed = float(self.brainz.speed)
else:
if self.cycle_time > float(self.brainz.speed_change_cycle):
self.cycle_time = 0
if self.cycle_time > int(self.brainz.speed_motors_full_percent) * float(self.brainz.speed_change_cycle) / 100.0:
new_speed = float(self.brainz.speed) * int(self.brainz.low_speed_percent) / 100.0
else:
new_speed = float(self.brainz.speed)
# Divide speed to two motors
new_right_motor_speed = min(1.0, new_speed * (1.0 - float(self.brainz.turn)))
new_left_motor_speed = min(1.0, new_speed * (1.0 + float(self.brainz.turn)))
self.rightMotor.run(Adafruit_MotorHAT.FORWARD)
self.leftMotor.run(Adafruit_MotorHAT.FORWARD)
self.leftMotor.setSpeed(int(new_left_motor_speed * 255) )
self.rightMotor.setSpeed(int(new_right_motor_speed * 255))
self.__print("Set speedright:" + str(int(new_right_motor_speed * 255)) + " " + str(int(new_left_motor_speed * 255)))
def __init__(self, with_rgb=True):
self.with_rgb = with_rgb
motorhat = Adafruit_MotorHAT(addr=0x60)
self.leftMotor = motorhat.getMotor(1)
self.rightMotor = motorhat.getMotor(2)
if self.with_rgb:
self.rgb = RGB_LED()
class Tracking:
def __init__(self):
self.node_name = rospy.get_name()
self.state = 1
self.trig = None
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.leftMotor = self.motorhat.getMotor(1)
self.rightMotor = self.motorhat.getMotor(2)
self.right_pwm = 120
self.left_pwm = 120
self.leftMotor.setSpeed(self.left_pwm)
self.rightMotor.setSpeed(self.right_pwm)
self.subPosition = rospy.Subscriber("/serial_node/odometry",
Float64MultiArray, self.cbPosition)
self.track = ''
rospy.on_shutdown(self.custom_shutdown)
rospy.loginfo("[%s] Initialized!" % self.node_name)
def cbPosition(self, msg):
x = msg.data[0]
y = msg.data[1]
theta = msg.data[2]
theta = theta % (2 * pi)
self.cbMove(x, y, theta)
print x, y, theta
def cbMove(self, x, y, theta):
if x < 0: self.track = 'stop'
elif x < 1: self.track = 'straight'
else: self.track = 'turn'
if self.track == 'straight':
self.straight()
elif self.track == 'turn':
self.turn_left(0.25)
elif self.track == 'stop':
self.custom_shutdown()
def straight(self):
self.leftMotor.setSpeed(self.left_pwm)
self.rightMotor.setSpeed(self.right_pwm)
self.leftMotor.run(1)
self.rightMotor.run(1)
def turn_left(self, radius):
b = 0.235 / 2
self.leftMotor.setSpeed(self.right_pwm * (radius - b) / (radius + b))
self.rightMotor.setSpeed(self.right_pwm)
self.leftMotor.run(1)
self.rightMotor.run(1)
def custom_shutdown(self):
self.leftMotor.run(4)
self.rightMotor.run(4)
del self.motorhat
def init(self):
left_motor_channel = 1
right_motor_channel = 2
driver = HAT(i2c_bus=1)
self.left_motor = driver.getMotor(left_motor_channel)
self.right_motor = driver.getMotor(right_motor_channel)
speed = 75 # 0 .. 255
self.left_motor.setSpeed(speed)
self.right_motor.setSpeed(speed)
self.attach_timer(0.1)
def main():
"""Releases any stepper motors"""
motor_hat = Adafruit_MotorHAT()
motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
output_json_message(
{'type': "update", 'status': "complete", 'msg': "Released Stepper Motors!"})
def __init__(self):
rospy.init_node("motor")
self.nodename = rospy.get_name()
rospy.loginfo("%s started" % self.nodename)
mh = Adafruit_MotorHAT(addr=0x60)
self.left = mh.getMotor(1)
self.right = mh.getMotor(2)
rospy.on_shutdown(self.motor_off)
atexit.register(self.motor_off)
rospy.Subscriber("left_motor_cmd", Float32, callback=self.on_left)
rospy.Subscriber("right_motor_cmd", Float32, callback=self.on_right)
class Rover: def __init__(self,address=0x60): self.mh = Adafruit_MotorHAT(addr=address) self.motors = [] for i in range(1,5): self.motors.append(self.mh.getMotor(i)) self.motors[i-1].setSpeed(0) def stop(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 setWheel(self, mot, val): if val<0: self.motors[mot].run(Adafruit_MotorHAT.BACKWARD) self.motors[mot].setSpeed(-1*val) else: self.motors[mot].run(Adafruit_MotorHAT.FORWARD) self.motors[mot].setSpeed(val) def releaseWheel(self, mot): self.motors[mot].run(Adafruit_MotorHAT.RELEASE) def move(self,left, right): if -50<left<50: left = 0 if -50<right<50: right = 0 self.setWheel(0, left) self.setWheel(1, left) self.setWheel(2, right) self.setWheel(3, right) def Joy2Mot(self, nJoyX, nJoyY, moveAlso=True): fPivYLimit = 32.0 if nJoyY>=0: nMotPremixL = 127.0 if (nJoyX>=0) else (127.0+nJoyX) nMotPremixR = (127.0-nJoyX) if (nJoyX>=0) else 127.0 else: nMotPremixL = (127.0-nJoyX) if (nJoyX>=0) else 127.0 nMotPremixR = 127.0 if (nJoyX>=0) else (127.0+nJoyX) nMotPremixL *= nJoyY/128.0 nMotPremixR *= nJoyY/128.0; nPivSpeed = nJoyX; fPivScale = 0.0 if (abs(nJoyY)>fPivYLimit) else (1.0-abs(nJoyY)/fPivYLimit) nMotMixL = (1.0-fPivScale)*nMotPremixL + fPivScale*nPivSpeed nMotMixR = (1.0-fPivScale)*nMotPremixR + fPivScale*-1*nPivSpeed left_wheel = int(nMotMixL*1.9921875) right_wheel = int(nMotMixR*1.9921875) if moveAlso: self.move(left_wheel,right_wheel) return [left_wheel,right_wheel]
class MotorsBase(object):
def __init__(self):
self.mh=Adafruit_MotorHAT(addr=0x60)
atexit.register(self.stop)
def stop(self):
for i in xrange(1,5):
self.mh.getMotor(i).run(Adafruit_MotorHAT.RELEASE)
def set_speed(self,motor,speed):
motor.setSpeed(speed)
class Unit:
def __init__(self):
self.mh = Adafruit_MotorHAT(addr=0x60)
self.motors = {}
self.setup()
def setup():
self.motors['fr'] = self.mh.getMotor(1)
self.motors['br'] = self.mh.getMotor(2)
self.motors['fl'] = self.mh.getMotor(3)
self.motors['bl'] = self.mh.getMotor(4)
class Interface:
"""
Abstract:
Simple script to baseline motor execution.
"""
def __init__(self):
self.LOGGER = Logger(self)
self.controller = Adafruit_MotorHAT(addr=0x60, i2c_bus=1)
self.motors = [0, 0, 0, 0]
# Initialize motor subscribers
rospy.init_node('interface')
rospy.Subscriber("/motor", String, self.on_motor_callback, queue_size=1)
# Turn off motors when the script exits.
atexit.register(self.turn_off_motors)
# Motor tracking
r = rospy.Rate(2)
while not rospy.is_shutdown():
self.LOGGER.info(str(self.motors))
r.sleep()
def turn_off_motors(self):
"""
Turns off all motors.
"""
self.LOGGER.info("Stopping motors...")
self.turn_motors([0, 0, 0, 0])
def turn_motors(self, values):
"""
ACTUALLY turns the motors.
"""
if self.motors == values or len(values) is not 4:
return
self.LOGGER.info("Motors turning: " + str(values))
while self.motors[0] != values[0] and self.motors[1] != values[1] and self.motors[2] != values[2] and self.motors[3] != values[3]:
for idx, motor_speed in enumerate(values):
direction = Adafruit_MotorHAT.FORWARD if self.motors[idx] > 0 else Adafruit_MotorHAT.BACKWARD if self.motors[idx] < 0 else Adafruit_MotorHAT.BRAKE
self.controller.getMotor(idx + 1).run(direction)
self.motors[idx] += (1 if self.motors[idx] < values[idx] else -1 if self.motors[idx] > values[idx] else 0)
self.controller.getMotor(idx + 1).setSpeed(int(abs(self.motors[idx])))
time.sleep(0.001)
def on_motor_callback(self, value_str_obj):
"""
Performs the actual motor operations.
"""
values = list(map(lambda x: float(x), value_str_obj.data.replace("\\", "").replace(" ", "").split(",")))
self.turn_motors(values)
self.motors = values
class Hat:
def __init__(self, hat):
self.addr = hat['addr']
self.freq = hat['freq']
self.mh = Adafruit_MotorHAT(addr=hat['addr'], freq=hat['freq'])
atexit.register(self.__turnOffMotors)
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 gopher(message):
if message[0:2] == 'mp':
# Initialze
mh = Adafruit_MotorHAT(addr=0x62)
# Get motor
motor = mh.getMotor(int(message[2]))
#Get direction and speed
dirSpeed = message[4:]
if dirSpeed[0] == '-':
direction = Adafruit_MotorHAT.BACKWARD
speed = int(dirSpeed[1:])
elif dirSpeed[0] == '0':
direction = Adafruit_MotorHAT.RELEASE
speed = 0
else:
direction = Adafruit_MotorHAT.FORWARD
speed = int(dirSpeed)
# Set direction
motor.run(direction)
#Set speed
motor.setSpeed(speed)
if message[0:2] == 'mt':
# Initialze
#from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
mh = Adafruit_MotorHAT(addr=0x62)
# Get motor
motor = mh.getMotor(int(message[2]))
#Get direction and time
dirTime = message[4:]
if dirTime[0:2] == '-1':
direction = Adafruit_MotorHAT.BACKWARD
time = int(dirTime[3:])
elif dirTime[0] == '1':
direction = Adafruit_MotorHAT.FORWARD
time = int(dirTime[2:])
# Set direction
motor.run(direction)
# Run for time
motor.setSpeed(255)
sleep(time)
motor.run(Adafruit_MotorHAT.RELEASE)
motor.setSpeed(0)
class Hat:
def __init__(self, hat):
self.addr = hat['addr']
self.freq = hat['freq']
self.mh = Adafruit_MotorHAT(addr=hat['addr'], freq=hat['freq'])
atexit.register(self.__turnOffMotors)
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)
class MotorControllerWaveshare(MotorController):
"""
Motor controller node that supports the Waveshare JetBot.
@see motors.py for the base class to implement different controllers.
"""
MOTOR_LEFT = 1 # left motor ID
MOTOR_RIGHT = 2 # right motor ID
def __init__(self):
super().__init__()
# open Adafruit MotorHAT driver
self.driver = Adafruit_MotorHAT(i2c_bus=1)
# get motor objects from driver
self.motors = {
self.MOTOR_LEFT : self.driver.getMotor(self.MOTOR_LEFT),
self.MOTOR_RIGHT : self.driver.getMotor(self.MOTOR_RIGHT)
}
self.pwm_channels = {
self.MOTOR_LEFT : (1, 0),
self.MOTOR_RIGHT : (2, 3)
}
def set_speed(self, left, right):
"""
Sets the motor speeds between [-1.0, 1.0]
"""
self._set_pwm(self.MOTOR_LEFT, left, self.left_trim)
self._set_pwm(self.MOTOR_RIGHT, right, self.right_trim)
def _set_pwm(self, motor, value, trim):
# apply trim and convert [-1,1] to PWM value
pwm = int(min(max((abs(value) + trim) * self.max_pwm, 0), self.max_pwm))
self.motors[motor].setSpeed(pwm)
# set the motor direction
ina, inb = self.pwm_channels[motor]
if value > 0:
self.motors[motor].run(Adafruit_MotorHAT.FORWARD)
self.driver._pwm.setPWM(ina, 0, pwm * 16)
self.driver._pwm.setPWM(inb, 0, 0)
elif value < 0:
self.motors[motor].run(Adafruit_MotorHAT.BACKWARD)
self.driver._pwm.setPWM(ina, 0, 0)
self.driver._pwm.setPWM(inb, 0, pwm * 16)
else:
self.motors[motor].run(Adafruit_MotorHAT.RELEASE)
self.driver._pwm.setPWM(ina, 0, 0)
self.driver._pwm.setPWM(inb, 0, 0)
class Motorhat(Block):
version = VersionProperty('0.1.0')
motor1_speed = FloatProperty(title='DC Motor 1 Speed', default=0)
motor2_speed = FloatProperty(title='DC Motor 2 Speed', default=0)
motor3_speed = FloatProperty(title='DC Motor 3 Speed', default=0)
motor4_speed = FloatProperty(title='DC Motor 4 Speed', default=0)
def configure(self, context):
super().configure(context)
self.MotorHAT = Adafruit_MotorHAT(addr=0x60)
def process_signals(self, signals):
for signal in signals:
for r in range(1, 5):
speed = getattr(self, 'motor{}_speed'.format(r))(signal)
direction = Adafruit_MotorHAT.FORWARD if speed >= 0 \
else Adafruit_MotorHAT.BACKWARD
self.MotorHAT.getMotor(r).run(direction)
self.MotorHAT.getMotor(r).setSpeed(abs(int(speed)))
def stop(self):
for r in range(1, 5):
direction = Adafruit_MotorHAT.FORWARD
self.MotorHAT.getMotor(r).run(direction)
self.MotorHAT.getMotor(r).setSpeed(0)
super().stop()
class r3c(object):
def __init__(self, addr=0x60, left_front_id=1, right_front_id=2,left_rear_id=3, right_rear_id=4, left_trim=0, right_trim=0,
stop_at_exit=True):
# Initialize motor HAT and left, right motor.
self._mh = Adafruit_MotorHAT(addr)
self._left_front = self._mh.getMotor(left_front_id)
self._right_front = self._mh.getMotor(right_front_id)
self._left_rear = self._mh.getMotor(left_rear_id)
self._right_rear = self._mh.getMotor(right_rear_id)
self._left_trim = left_trim
self._right_trim = right_trim
# Start with motors turned off.
self._left_front.run(Adafruit_MotorHAT.RELEASE)
self._right_front.run(Adafruit_MotorHAT.RELEASE)
self._left_rear.run(Adafruit_MotorHAT.RELEASE)
self._right_rear.run(Adafruit_MotorHAT.RELEASE)
# Configure all motors to stop at program exit if desired.
if stop_at_exit:
atexit.register(self.stop)
def nav(self, left_speed, right_speed, run_duration=5):
move_command = get_move_command(left_speed, right_speed)
run_start_time = time.time()
while((time.time() - run_start_time) <= run_duration):
self.move(left_speed, right_speed)
print "navigating: all good.. "
if(get_dist('fc') < 5 and (('FWD' in move_command) or ('PIVOT' in move_command))):
print "obstacle!"
self.pause()
self.nav(-80,-80,2)
self.pivot(5)
self.stop()
print "run time up!"
def get_move_command(self, left_speed, right_speed):
move_command = 'UNKNOWN'
if(left_speed > 0 and right_speed > 0):
if(left_speed == right_speed):
move_command = 'FWD'
if(left_speed > right_speed):
move_command = 'FWD_RIGHT'
if(left_speed < right_speed):
move_command = 'FWD_LEFT'
elif(left_speed < 0 and right_speed < 0):
move_command = 'RVRS'
elif(left_speed = 0 and right_speed > 0):
move_command = 'PIVOT_LEFT'
elif(left_speed > 0 and right_speed < 0):
move_command = 'PIVOT_RIGHT'
class Adafruit_Motor_Hat_Controller:
'''
Adafruit DC Motor Controller
For differential drive cars you need one controller for each motor.
'''
def __init__(self, motor_num):
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import atexit
self.FORWARD = Adafruit_MotorHAT.FORWARD
self.BACKWARD = Adafruit_MotorHAT.BACKWARD
self.mh = Adafruit_MotorHAT(addr=0x60)
self.motor = self.mh.getMotor(motor_num)
self.motor_num = motor_num
atexit.register(self.turn_off_motors)
self.speed = 0
self.throttle = 0
def turn_off_motors(self):
self.mh.getMotor(self.motor_num).run(Adafruit_MotorHAT.RELEASE)
def turn(self, speed):
'''
Update the speed of the motor where 1 is full forward and
-1 is full backwards.
'''
if speed > 1 or speed < -1:
raise ValueError(
"Speed must be between 1(forward) and -1(reverse)")
self.speed = speed
self.throttle = int(map_range(abs(speed), -1, 1, -255, 255))
if speed > 0:
self.motor.run(self.FORWARD)
else:
self.motor.run(self.BACKWARD)
self.motor.setSpeed(self.throttle)
def test(self, seconds=.5):
speeds = [-.5, -1, -.5, 0, .5, 1, 0]
for s in speeds:
self.turn(s)
time.sleep(seconds)
print('speed: %s throttle: %s' % (self.speed, self.throttle))
print('motor #%s test complete' % self.motor_num)
def main():
rospy.init_node('me212bot', anonymous=True)
if DEMO == 1:
serialComm = serial.Serial('/dev/ttyACM0', 115200, timeout=5)
Motorhat = Adafruit_MotorHAT(addr=0x60)
leftMotor = Motorhat.getMotor(1)
rightMotor = Motorhat.getMotor(2)
odometry_thread = threading.Thread(target=read_odometry_loop)
odometry_thread.start()
## 1. Initialize a subscriber (subscribe ROS topic)
cmdvel_sub = rospy.Subscriber('/cmdvel', WheelCmdVel, cmdvel_callback)
rospy.spin()
def __init__(self):
driver = Adafruit_MotorHAT(i2c_bus=self.I2C_BUS)
self.left_motor = driver.getMotor(1)
self.right_motor = driver.getMotor(2)
self.vertical_motor = driver.getMotor(3)
# monitor
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.UP_LIMIT_PIN, GPIO.IN)
GPIO.setup(self.DOWN_LIMIT_PIN, GPIO.IN)
GPIO.add_event_detect(self.UP_LIMIT_PIN,
GPIO.FALLING,
callback=self.vertical_limit_callback)
GPIO.add_event_detect(self.DOWN_LIMIT_PIN,
GPIO.FALLING,
callback=self.vertical_limit_callback)
class gazebo_car_control_node(object):
def __init__(self):
self.node_name = "gazebo_car_control_node"
self.active = True
self.i = 0
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.gazebo_car_control_L = self.motorhat.getMotor(1)
self.gazebo_car_control_R = self.motorhat.getMotor(2)
self.threshold = 0
self.sub_control_value = rospy.Subscriber("/gazebo_sub_jointstate/control_value", Point, self.cbControl_value, queue_size=1)
self.sub_threshold_value = rospy.Subscriber("/gazebo_sub_jointstate/threshold_value", Int64, self.cbThreshold_value, queue_size=1)
def cbThreshold_value(self, msg):
self.threshold = msg.data
def cbControl_value(self, msg):
u_R = int(msg.x)
u_L = int(msg.y)
print "u_R = ",u_R,"\tu_L = ",u_L
# if u_R > 200:
# u_R = 200
# if u_L > 200:
# u_L = 200
# if u_R < -200:
# u_R = -200
# if u_L < -200:
# u_L = -200
if u_R > self.threshold:
self.gazebo_car_control_R.setSpeed(u_R)
self.gazebo_car_control_R.run(Adafruit_MotorHAT.FORWARD)
elif u_R < -(self.threshold):
self.gazebo_car_control_R.setSpeed(-u_R)
self.gazebo_car_control_R.run(Adafruit_MotorHAT.BACKWARD)
else:
self.gazebo_car_control_R.setSpeed(0)
self.gazebo_car_control_R.run(Adafruit_MotorHAT.BACKWARD)
if u_L > self.threshold:
self.gazebo_car_control_L.setSpeed(u_L)
self.gazebo_car_control_L.run(Adafruit_MotorHAT.FORWARD)
elif u_L < -(self.threshold):
self.gazebo_car_control_L.setSpeed(-u_L)
self.gazebo_car_control_L.run(Adafruit_MotorHAT.BACKWARD)
else:
self.gazebo_car_control_L.setSpeed(0)
self.gazebo_car_control_L.run(Adafruit_MotorHAT.FORWARD)
# self.gazebo_car_control_.run(Adafruit_MotorHAT.RELEASE)
def onShutdown(self):
self.gazebo_car_control_R.setSpeed(0)
self.gazebo_car_control_R.run(Adafruit_MotorHAT.BACKWARD)
self.gazebo_car_control_L.setSpeed(0)
self.gazebo_car_control_L.run(Adafruit_MotorHAT.FORWARD)
rospy.loginfo("[gazebo_car_control_node] Shutdown.")
def release_motors(use_dummy=False):
"""Releases any stepper motors"""
if not use_dummy:
from Adafruit_MotorHAT import Adafruit_MotorHAT
motor_hat = Adafruit_MotorHAT()
motor_hat.getMotor(1).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(2).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(3).run(Adafruit_MotorHAT.RELEASE)
motor_hat.getMotor(4).run(Adafruit_MotorHAT.RELEASE)
output_json_message({
'type': "update",
'status': "progress",
'msg': "Released Stepper Motors!"
})
time.sleep(0.1)
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 __init__(self):
"""
Creates motor object attributes.
"""
mh = Adafruit_MotorHAT(addr=0x60)
self.motors = tuple(mh.getMotor(n) for n in (1, 2, 3, 4))
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"
def motor_setup(): """Get pointers to both of the motors and to the hat""" global _MOTORHAT _MOTORHAT = Adafruit_MotorHAT(addr=0x60) global _DRIVE_MOTOR1 global _DRIVE_MOTOR2 _DRIVE_MOTOR1 = _MOTORHAT.getMotor(1) _DRIVE_MOTOR2 = _MOTORHAT.getMotor(2)
class Robot(object):
def __init__(self, left_channel, right_channel):
self.motor = Adafruit_MotorHAT(0x60)
self.left_motor = self.motor.getMotor(left_channel)
self.right_motor = self.motor.getMotor(right_channel)
def set_speed(self):
self.left_motor.setSpeed(200)
self.right_motor.setSpeed(200)
def stop(self):
self.left_motor.run(Adafruit_MotorHAT.RELEASE)
self.right_motor.run(Adafruit_MotorHAT.RELEASE)
def forward(self, duration):
self.set_speed()
self.left_motor.run(Adafruit_MotorHAT.FORWARD)
self.right_motor.run(Adafruit_MotorHAT.FORWARD)
time.sleep(duration)
self.stop()
def reverse(self, duration):
self.set_speed()
self.left_motor.run(Adafruit_MotorHAT.BACKWARD)
self.right_motor.run(Adafruit_MotorHAT.BACKWARD)
time.sleep(duration)
self.stop()
def left(self, duration):
self.set_speed()
self.right_motor.run(Adafruit_MotorHAT.FORWARD)
time.sleep(duration)
self.stop()
def right(self, duration):
self.set_speed()
self.left_motor.run(Adafruit_MotorHAT.FORWARD)
time.sleep(duration)
self.stop()
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 Motors(object):
left_motor_id = 1
right_motor_id = 2
left_motor_trim = 0
right_motor_trim = 0
def __init__(self, motor_controller_i2c_address = 0x60):
self.motor_controller = Adafruit_MotorHAT(motor_controller_i2c_address)
self.left_motor = self.motor_controller.getMotor(self.left_motor_id)
self.right_motor = self.motor_controller.getMotor(self.right_motor_id)
def boot(self):
# stop both motors
self.all_stop()
atexit.register(self.all_stop())
def all_stop(self):
self.left_motor.run(Adafruit_MotorHAT.RELEASE)
self.right_motor.run(Adafruit_MotorHAT.RELEASE)
def move(self, direction, speed, for_seconds=None):
self.left_motor.setSpeed( max(0, min(speed + self.left_motor_trim, 255)))
self.right_motor.setSpeed(max(0, min(speed + self.right_motor_trim, 255)))
motor_directions = {
' forward': {'left': Adafruit_MotorHAT.FORWARD, 'right': Adafruit_MotorHAT.FORWARD },
'backward': {'left': Adafruit_MotorHAT.BACKWARD, 'right': Adafruit_MotorHAT.BACKWARD},
'left': {'left': Adafruit_MotorHAT.BACKWARD, 'right': Adafruit_MotorHAT.FORWARD },
'right': {'left': Adafruit_MotorHAT.FORWARD, 'right': Adafruit_MotorHAT.BACKWARD},
}[direction]
self.left_motor.run(motor_directions['left'])
self.right_motor.run(motor_directions['right'])
if for_seconds is not None:
time.sleep(for_seconds)
self.all_stop()
class DCMotorController:
def __init__(self, num):
self._number = num
self.mh = Adafruit_MotorHAT() #Open motor hat instance
self.motor = self.mh.getMotor(self._number)
def stop_motor (self):
self.motor.run(MtrHat.Adafruit_MotorHAT.RELEASE)
def start_motor (self, contr_handle, duty_cycle, motor_dir):
self.motor.setSpeed(duty_cycle)
self.motor.run(motor_dir)
logging.info("Heating at: {}% Duty Cycle".format(duty_cycle))
def __init__(self): self.Del = 0.01 self.Imname = '/var/www/webcam.jpg' self.State = [0,0,0,0] self.FrontBumpPin = 18 self.RearBumpPin = 7 self.ResX = 800 self.ResY = 600 GPIO.setup(self.FrontBumpPin, GPIO.IN, pull_up_down=GPIO.PUD_UP) GPIO.setup(self.RearBumpPin, GPIO.IN, pull_up_down=GPIO.PUD_UP) # create a default object, no changes to I2C address or frequency mh = Adafruit_MotorHAT(addr=0x60) self.LFMotor = mh.getMotor(1) self.LRMotor = mh.getMotor(2) self.RRMotor = mh.getMotor(3) self.RFMotor = mh.getMotor(4) # turn on motor self.RFMotor.run(Adafruit_MotorHAT.RELEASE); self.LFMotor.run(Adafruit_MotorHAT.RELEASE); self.RRMotor.run(Adafruit_MotorHAT.RELEASE); self.LRMotor.run(Adafruit_MotorHAT.RELEASE);
class dc_grab(object):
def __init__(self):
self.node_name = "dc_grab"
self.active = True
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.dc_grab = self.motorhat.getMotor(3)
self.sub_joy_ = rospy.Subscriber("joy", Joy, self.cbJoy, queue_size=1)
self.pub_gazebo_grab = rospy.Publisher('/duckiebot_with_gripper/gripper_cmd_vel', Twist, queue_size=1)
def cbJoy(self, msg):
self.joy = msg
self.processButtons(msg)
def processButtons(self, msg):
grab_state_msg = Twist()
grab_state_msg.linear.x = 0
grab_state_msg.linear.y = 0
grab_state_msg.linear.z = 0
grab_state_msg.angular.x = 0
grab_state_msg.angular.y = 0
grab_state_msg.angular.z = 0
if (self.joy.buttons[0] == 1):
self.dc_grab.setSpeed(200)
self.dc_grab.run(Adafruit_MotorHAT.BACKWARD)
grab_state_msg.angular.z = -1
self.pub_gazebo_grab.publish(grab_state_msg)
if (self.joy.buttons[1] == 1):
self.dc_grab.setSpeed(200)
self.dc_grab.run(Adafruit_MotorHAT.FORWARD)
grab_state_msg.angular.z = +1
self.pub_gazebo_grab.publish(grab_state_msg)
if (self.joy.buttons[2] == 1):
self.dc_grab.run(Adafruit_MotorHAT.RELEASE)
def onShutdown(self):
rospy.loginfo("[dc_grab] Shutdown.")
class motor(QThread):
def __init__(self):
self.mySpeed = 100
#print ("initial motor at "+str(mySpeed))
print ("initial motor at "+str(self.mySpeed))
self.mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(self.turnOffMotors)
self.myMotor_front = self.mh.getMotor(1)
self.myMotor_back = self.mh.getMotor(4)
#self.myMotor_front.setSpeed(mySpeed)
def turnOffMotors(self):
print "Off Motor Complete"
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 left(self):
self.myMotor_front.run(Adafruit_MotorHAT.FORWARD)
self.myMotor_back.run(Adafruit_MotorHAT.FORWARD)
print "\tSpeed up...to"+str(self.mySpeed)
self.myMotor_front.setSpeed(self.mySpeed)
self.myMotor_back.setSpeed(self.mySpeed)
time.sleep(0.01)
def right(self):
self.myMotor_front.run(Adafruit_MotorHAT.BACKWARD)
self.myMotor_back.run(Adafruit_MotorHAT.BACKWARD)
print "\tSpeed up...to"+str(self.mySpeed)
self.myMotor_front.setSpeed(self.mySpeed)
self.myMotor_back.setSpeed(self.mySpeed)
time.sleep(0.01)
def stop(self):
self.turnOffMotors()
def setmySpeed(self, speed):
self.turnOffMotors()
print ("new set speed = "+str(speed))
#self.mySpeed = speed
self.mySpeed = int(speed)
# allow the camera to warmup
time.sleep(0.1)
firstFrame = None
mode = 1
textControl = None
checkMode2 = None
mySpeed = 255
#intital motor
print ("initial motor at "+str(mySpeed))
mh = Adafruit_MotorHAT(addr=0x60)
atexit.register(turnOffMotors)
myMotor_front = mh.getMotor(1)
myMotor_back = mh.getMotor(4)
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
if mode == 1:
textMode = "Object Detection"
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
image = imutils.resize(image, width=480)
text = "Not Ready"
fgmask = fgbg.apply(image)
# show the frame
#cv2.imshow("Original", image)
#cv2.imshow("Mask", fgmask)
class DaguWheelsDriver:
LEFT_MOTOR_MIN_PWM = 60 # Minimum speed for left motor
LEFT_MOTOR_MAX_PWM = 255 # Maximum speed for left motor
RIGHT_MOTOR_MIN_PWM = 60 # Minimum speed for right motor
RIGHT_MOTOR_MAX_PWM = 255 # Maximum speed for right motor
# AXEL_TO_RADIUS_RATIO = 1.0 # The axel length and turning radius ratio
SPEED_TOLERANCE = 1.e-2; # speed tolerance level
def __init__(self, verbose=False, debug=False, left_flip=False, right_flip=False):
self.motorhat = Adafruit_MotorHAT(addr=0x60)
self.leftMotor = self.motorhat.getMotor(1)
self.rightMotor = self.motorhat.getMotor(2)
self.verbose = verbose or debug
self.debug = debug
self.left_sgn = 1.0;
if left_flip:
self.left_sgn = -1.0;
self.right_sgn = 1.0;
if right_flip:
self.right_sgn = -1.0;
self.leftSpeed = 0.0
self.rightSpeed = 0.0
self.updatePWM()
def PWMvalue(self, v, minPWM, maxPWM):
pwm = 0;
if fabs(v) > self.SPEED_TOLERANCE:
pwm = int(floor(fabs(v) * (maxPWM - minPWM) + minPWM))
return min(pwm,maxPWM);
def updatePWM(self):
vl = self.leftSpeed*self.left_sgn
vr = self.rightSpeed*self.right_sgn
pwml = self.PWMvalue(vl, self.LEFT_MOTOR_MIN_PWM, self.LEFT_MOTOR_MAX_PWM)
pwmr = self.PWMvalue(vr, self.RIGHT_MOTOR_MIN_PWM, self.RIGHT_MOTOR_MAX_PWM)
if self.debug:
print "v = %5.3f, u = %5.3f, vl = %5.3f, vr = %5.3f, pwml = %3d, pwmr = %3d" % (v, u, vl, vr, pwml, pwmr)
if fabs(vl) < self.SPEED_TOLERANCE:
leftMotorMode = Adafruit_MotorHAT.RELEASE
elif vl > 0:
leftMotorMode = Adafruit_MotorHAT.FORWARD
elif vl < 0:
leftMotorMode = Adafruit_MotorHAT.BACKWARD
if fabs(vr) < self.SPEED_TOLERANCE:
rightMotorMode = Adafruit_MotorHAT.RELEASE
pwmr = 0;
elif vr > 0:
rightMotorMode = Adafruit_MotorHAT.FORWARD
elif vr < 0:
rightMotorMode = Adafruit_MotorHAT.BACKWARD
self.leftMotor.setSpeed(pwml)
self.leftMotor.run(leftMotorMode);
self.rightMotor.setSpeed(pwmr)
self.rightMotor.run(rightMotorMode);
def setWheelsSpeed(self,left,right):
self.leftSpeed = left
self.rightSpeed = right
self.updatePWM()
def __del__(self):
self.leftMotor.run(Adafruit_MotorHAT.RELEASE)
self.rightMotor.run(Adafruit_MotorHAT.RELEASE)
del self.motorhat
socket = soc.socket(soc.AF_INET, soc.SOCK_STREAM)
address = ('localhost', 5432) # Create an address tuple
socket.bind(address)
# 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)
motorA = mh.getMotor(1)
motorB = mh.getMotor(3)
motorA.run(Adafruit_MotorHAT.FORWARD)
motorB.run(Adafruit_MotorHAT.FORWARD)
while 1: # This will loop forever
socket.listen(1)
print "Robot has connected"
connection, addrress = socket.accept() # The program blocks here
while 1: # While somebody is connected
data = connection.recv(1024)
if len(data) == 0:
print "Disconnected..."
break
else:
class motors():
def __init__(self):
self.mh = Adafruit_MotorHAT(addr=0x70) #note stopping this program DOES NOT stop the motor
#we COULD make this an array but for programming simplicity, have 4 variables
self.upperLeft=self.mh.getMotor(1)
self.upperRight=self.mh.getMotor(2)
self.lowerLeft=self.mh.getMotor(3)
self.lowerRight=self.mh.getMotor(4)
self.motorSpeed=[0,0]
#might have it work similarly to forward
def back(self,speed =100,accel=20):
'''
Input:
speed: a number from 1-255 which sends a signal to the motor hat to the DC motor to a certain speed
accel: how fast do you want to reach the max speed
NOTE: -1 is a special code that tells me that you want to get to maximum speed immediately, it was a hack
Output:
The motors move that speed
'''
if speed > 255 or speed < 0:
return # do nothing for now, write error message at some point
#initialize the direction of the motors
self.upperLeft.run(Adafruit_MotorHAT.FORWARD)
self.upperRight.run(Adafruit_MotorHAT.FORWARD)
self.lowerLeft.run(Adafruit_MotorHAT.FORWARD)
self.lowerRight.run(Adafruit_MotorHAT.FORWARD)
#slowly accellerate the motors so they aren't jerky,
#think of someone constantly stopping and starting car
for i in range(speed)[::accel]:
self.upperLeft.setSpeed(i)
self.upperRight.setSpeed(i)
self.lowerLeft.setSpeed(i)
self.lowerRight.setSpeed(i)
#these are assumed to be at a full stop
def turnRight(self,speed =5, angle=30, accel=20):
'''
Input:
speed: a number from 1-255 which sends a signal to the motor hat to the DC motor to a certain speed
accel: how fast do you want to reach the max speed
Output:
The motors move that speed
'''
if speed > 255 or speed < 0:
return # do nothing for now, write error message at some point
self.upperLeft.run(Adafruit_MotorHAT.BACKWARD)
self.upperRight.run(Adafruit_MotorHAT.FORWARD)
self.lowerLeft.run(Adafruit_MotorHAT.BACKWARD)
self.lowerRight.run(Adafruit_MotorHAT.FORWARD)
#make sure the library is imported from the compass
for i in range(speed)[::accel]:
self.upperLeft.setSpeed(i)
self.upperRight.setSpeed(i)
self.lowerLeft.setSpeed(i)
self.lowerRight.setSpeed(i)
def turnLeft(self,speed =5,accel=10):
'''
Input:
speed: a number from 1-255 which sends a signal to the motor hat to the DC motor to a certain speed
accel: how fast do you want to reach the max speed
NOTE: -1 is a special code that tells me that you want to get to maximum speed immediately, it was a hack
Output:
The motors move that speed
'''
#same as turn left, figure out Radians or Degrees
if speed > 255 or speed < 0:
return # do nothing for now, write error message at some point
self.upperLeft.run(Adafruit_MotorHAT.FORWARD)
self.upperRight.run(Adafruit_MotorHAT.BACKWARD)
self.lowerLeft.run(Adafruit_MotorHAT.FORWARD)
self.lowerRight.run(Adafruit_MotorHAT.BACKWARD)
if accel != -1:
for i in range(speed)[::accel]:
self.upperLeft.setSpeed(i)
self.upperRight.setSpeed(i)
self.lowerLeft.setSpeed(i)
self.lowerRight.setSpeed(i)
else:
self.upperLeft.setSpeed(speed)
self.upperRight.setSpeed(speed)
self.lowerLeft.setSpeed(speed)
self.lowerRight.setSpeed(speed)
def forward(self,speed =255,accel=10,driftRatio=1):
'''
Input:
speed: a number from 1-255 which sends a signal to the motor hat to the DC motor to a certain speed
accel: how fast do you want to reach the max speed
NOTE: -1 is a special code that tells me that you want to get to maximum speed immediately, it was a hack
driftRatio: a number between 1 and 2, honestly it could be larger but the math is hacky that essentially says Move right motors faster than the others
Output:
The motors move that speed
'''
if speed > 255 or speed < 0:
return # do nothing for now, write error message at some point
self.upperLeft.run(Adafruit_MotorHAT.BACKWARD)
self.upperRight.run(Adafruit_MotorHAT.BACKWARD)
self.lowerLeft.run(Adafruit_MotorHAT.BACKWARD)
self.lowerRight.run(Adafruit_MotorHAT.BACKWARD)
if driftRatio ==1:
#note that the Math
for i in range(motorSpeed[0],speed*driftRatio)[::accel]:
self.upperRight.setSpeed(i)
self.lowerRight.setSpeed(i)
for i in range(motorSpeed[1],speed)[::accel]:
self.upperLeft.setSpeed(i)
self.lowerLeft.setSpeed(i)
#for now, just for left/right control
self.motorSpeed=[speed,speed]
else:
#make sure values are valid
if speed*driftRatio > 255:
return
#hacky solution, try to have one set of wheels accellerate slightly faster than the other wheels
for i in range(motorSpeed[0],speed*driftRatio)[::accel]:
self.upperRight.setSpeed(i)
self.lowerRight.setSpeed(i)
for i in range(motorSpeed[1],speed)[::accel]:
self.upperLeft.setSpeed(i)
self.lowerLeft.setSpeed(i)
self.motorSpeed=[speed*driftRatio,speed]
def stop(self,speed=0):
'''
Input:
speed: a number from 1-255 which sends a signal to the motor hat to the DC motor to signify what speed it was moving
Output:
The motors stop
'''
#decellerates, then stops
for i in range(speed)[::-1]:
self.upperLeft.setSpeed(i)
self.upperRight.setSpeed(i)
self.lowerLeft.setSpeed(i)
self.lowerRight.setSpeed(i)
self.upperLeft.run(Adafruit_MotorHAT.RELEASE)
self.upperRight.run(Adafruit_MotorHAT.RELEASE)
self.lowerLeft.run(Adafruit_MotorHAT.RELEASE)
self.lowerRight.run(Adafruit_MotorHAT.RELEASE)
# optional sleep to make reduce jerkiness
#time.sleep(1.0)
def redirect(self,direction):
# quick 90 degree turn (will need some code to figure out which one) need to enable sensors for detection
# back max speed (unless a collision
# another quick 90 degree Turn to rorganise then zoom past to get behind it
self.turnRight(speed=50, angle=90)
self.back(speed=90)
self.turnRight(speed=50, angle=90)
self.forward(speed=50)
def movementTest():
try:
forward(25)
time.sleep(1)
stop()
#back(255)
#stop()
#turnLeft(255)
#stop()
#slow and smooth
#turnRight(speed=255, accel=1)
#fast and jerky
#turnRight(speed=255, accel=-1)
#oneWheelTime(speed=255, accel=10)
#time.sleep(1)
#stop()
finally:
stop()
from decimal import Decimal import time import atexit mh = Adafruit_MotorHAT(addr=0x60) 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) motor2 = mh.getMotor(2) myMotor = mh.getMotor(3) def mixingWater(name, size, mixTime): motor2.setSpeed(255) motor2.run(Adafruit_MotorHAT.RELEASE) motor2.run(Adafruit_MotorHAT.FORWARD) time.sleep(mixTime) print "MOTOR OFF!!" turnOffMotors() def mixTheDrink(name, size, mixTime): myMotor.setSpeed(255)
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);
stepstyles = [Adafruit_MotorHAT.SINGLE, Adafruit_MotorHAT.DOUBLE, Adafruit_MotorHAT.INTERLEAVE]
steppers = [myStepper1, myStepper2, myStepper3]
def stepper_worker(stepper, numsteps, direction, style):
#print("Steppin!")
stepper.step(numsteps, direction, style)
#print("Done")
while (True):
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
import sys # 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) ################################# DC motor test! myMotor1 = mh.getMotor(1) myMotor2 = mh.getMotor(2) myMotor3 = mh.getMotor(3) myMotor4 = mh.getMotor(4) arg1=sys.argv[1] arg2=int(sys.argv[2]) #print "arg1="+sys.argv[1] wheel=arg1 speed=20*arg2 print "wheel=%s speed=%d" % (wheel, speed) if speed>0:
import atexit # 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) ################################# DC motor test! myMotor = mh.getMotor(3) # set the speed to start, from 0 (off) to 255 (max speed) myMotor.setSpeed(150) myMotor.run(Adafruit_MotorHAT.FORWARD); # turn on motor myMotor.run(Adafruit_MotorHAT.RELEASE); while (True): print "Forward! " myMotor.run(Adafruit_MotorHAT.FORWARD) print "\tSpeed up..." for i in range(255): myMotor.setSpeed(i)
import os # 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) fl = mh.getMotor(1) fr = mh.getMotor(2) bl = mh.getMotor(3) br = mh.getMotor(4) motors = [fl, fr, bl, br] GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup(17, GPIO.IN) GPIO.setup(27, GPIO.IN) GPIO.setup(22, GPIO.IN) GPIO.setup(4, GPIO.IN) GPIO.setup(23, GPIO.IN)
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 mh = Adafruit_MotorHAT(addr=0x60) # auto-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) mL = mh.getMotor(1) mR = mh.getMotor(2) #====================================================================== # General Functions # (Both versions) # # init(). Initialises GPIO pins, switches motors and LEDs Off, etc # cleanup(). Sets all motors and LEDs off and sets GPIO to standard values # version(). Returns 1 for Full pi2kf, and 2 for pi2kf-Lite. Invalid until after init() has been called #====================================================================== #====================================================================== # Motor Functions # (Both Versions) #
class tBMotorController():
def __init__(self, motors):
self.mh = Adafruit_MotorHAT(addr=0x60)
allmotors=[1,2,3,4]
self.user_motors=motors;
self.motor = {}
self.motor.fromkeys(allmotors);
for (i,x) in enumerate(self.user_motors):
print"Initializing Motor %s" % x
if (x < 1 or x > 4):
print "ID out of range"
return (False)
self.motor[x] = self.mh.getMotor(x)
#self.sensorq = sensorq
self.haltRequest = False
self.speed = 0;
#TODO
#Run thread to get encoders from Arduino
#self.encoderq = Queue.Queue()
#tBEncoders = tBEncoderCapture("encoder-capture-thread", encoderq)
def setDirection(self, mId, direction):
# Set the direction
if direction == "FORWARD":
print "Driving Forward Motor %s " % mId
self.motor[mId].run(Adafruit_MotorHAT.FORWARD)
elif direction == "REVERSE":
print "Driving Backward Motor %s " % mId
self.motor[mId].run(Adafruit_MotorHAT.BACKWARD)
else:
print "Invalid direction"
return (False)
def runMotor(self, mId, speed):
self.motor[mId].setSpeed(speed)
#print "In run motor 2"
#wheel_speed = self.encoderq.get()
#print "In run motor 3"
#print "Speed = %s" % wheel_speed
#previous_error = 0
#integral = 0
#PID controoler when everything is ready
#while(1)
#get time
#error = setpoint - wheel_speed
#integral = integral + error*dt
#derivative = (error - previous_error)/dt
#output = Kp*error + Ki*integral + Kd*derivative
#previous_error = error
def stopMotors(self):
print("Attemping to stop all motors")
for mId in self.user_motors:
self.motor[mId].run(Adafruit_MotorHAT.RELEASE)
# recommended for auto-disabling motors on shutdown!
def turnOffMotor(self, mId):
print "Releasing motor %s" % mId
self.motor[mId].run(Adafruit_MotorHAT.RELEASE)
def cleanClose():
self.stopMotors()
#Adafruit IO
ADAFRUIT_IO_KEY = '954a29c4a56787437186d8c39c57a61d6c079867'
aio = Client(ADAFRUIT_IO_KEY)
data = aio.receive('Omnibot')
GPIO.setmode(GPIO.BCM)
#Thermal Printer
printer = Adafruit_Thermal("/dev/ttyAMA0", 19200, timeout=5)
#pwm
pwm2 = PWM(0x70)
#Motor Config
motor = Adafruit_MotorHAT(addr=0x63)
RMotor = motor.getMotor(1)
LMotor = motor.getMotor(2)
RMotor.setSpeed(50)
LMotor.setSpeed(50)
##MCP GPIO Pin Config##
#LCD
lcd_address = 0x20
gpio_lcd = 8 # Number of GPIOs exposed by the MCP230xx chip, should be 8 or 16 depending on chip.
mcp_lcd = MCP230XX_GPIO(1, lcd_address, gpio_lcd)
lcd = Adafruit_CharLCD(pin_rs=1, pin_e=2, pins_db=[3,4,5,6], GPIO=mcp_lcd)
# MCP23017 General
mcp23017_address = 0x23 # I2C address of the MCP230xx chip.
mcp_gpio = Adafruit_MCP230XX(mcp23017_address, 16)
#lcd colors
from datetime import date
# 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)
################################# DC motor test!
Motor1 = mh.getMotor(1)
Motor2 = mh.getMotor(2)
Motor3 = mh.getMotor(3)
Motor4 = mh.getMotor(4)
###################
##### Methods #####
###################
def UpdateDeviceStatus():
"""
Reads the current state of each device connected to the hub, and
updates the program's internal representation of that state.
"""
UpdateDoorStatus()
UpdateThermostatStatus()
UpdateLightStatus()
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)
# network functions go here
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect(('192.168.1.6', 9000))
while True:
response = s.recv(1024).decode("utf-8")
for r in response.split('\r\n'):
if r.startswith("MOVE"): #move the arm
#get the arguments
args = r.split(' ')
motor = mh.getMotor(int(args[1]))
if args[2] == 'FW':
direction = Adafruit_MotorHAT.FORWARD
else:
direction = Adafruit_MotorHAT.BACKWARD
speed = int(args[3])
#move the motor
motor.setSpeed(speed)
motor.run(direction)
time.sleep(0.8)
motor.run(Adafruit_MotorHAT.RELEASE)
