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)
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.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..")
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)
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 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)
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 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)
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 __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)
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 __init__(self, addr=0x60, left_id=3, right_id=4, left_trim=0, right_trim=0,
stop_at_exit=True):
"""Create an instance of the robot. Can specify the following optional
parameters:
- addr: The I2C address of the motor HAT, default is 0x60.
- left_id: The ID of the left motor, default is 1.
- right_id: The ID of the right motor, default is 2.
- left_trim: Amount to offset the speed of the left motor, can be positive
or negative and use useful for matching the speed of both
motors. Default is 0.
- right_trim: Amount to offset the speed of the right motor (see above).
- stop_at_exit: Boolean to indicate if the motors should stop on program
exit. Default is True (highly recommended to keep this
value to prevent damage to the bot on program crash!).
"""
# Initialize motor HAT and left, right motor.
self._mh = Adafruit_MotorHAT(addr)
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._left.run(Adafruit_MotorHAT.RELEASE)
self._right.run(Adafruit_MotorHAT.RELEASE)
# Configure all motors to stop at program exit if desired.
if stop_at_exit:
atexit.register(self.stop)
def __init__(self, verbose=False, debug=False, left_flip=False, right_flip=False, car_like_mode=True):
self.car_like_mode = car_like_mode
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.speed = 0.0
self.angle = 0.0
self.leftSpeed = 0.0
self.rightSpeed = 0.0
self.vmax = 1.0 / (1.0 + 0.5 * self.AXEL_TO_RADIUS_RATIO)
self.updatePWM()
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)
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]
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 __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("/kaku/car_cmd", Point, self.cbControl_value, queue_size=1)
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 __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)
def __init__(self): self.mh = Adafruit_MotorHAT() self.stepper = self.mh.getStepper(200,1) self.stepper.setSpeed(10) self.servo = PWM.Servo() self.SERVO_90 = 2400 self.SERVO_0 = 600 self.current_us = 1900 self.servo.move_servo(18,current_us) time.sleep(1)
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 __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 __init__(self, addr, motorlist, verbose=False):
self.name = addr
self.verbose = verbose
self.hat = Adafruit_MotorHAT(int(addr, 16))
self.motors = dict([(m, self.hat.getMotor(m)) for m in motorlist])
self.until = dict([(m, None) for m in motorlist])
for motorname, motor in self.motors.items():
motor.setSpeed(150)
motor.run(Adafruit_MotorHAT.FORWARD)
motor.run(Adafruit_MotorHAT.RELEASE)
if self.verbose:
print("init hat %s motor %s" % (self.name, motorname))
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 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 RaspberryPiMotor(Motor):
'''
Encapsulates communication to an underlying Adafruit motor HAT.
'''
def __init__(self, stepsPerRev=200):
initStep = 0
motorNum = 1
super(RaspberryPiMotor, self).__init__(initStep, stepsPerRev)
self.motor = Adafruit_MotorHAT(addr=0x70).getStepper(stepsPerRev, motorNum)
def __executeSteps(self, numSteps, stepType, direction, time_ms, updateSteps='20'):
self.setIsRunning(True)
sign = 1
if Motor.BACKWARD == direction:
sign = -1
self.motor.setSpeed(float(numSteps) / self.getStepsPerRevolution() / time_ms * 1000.0 * 60)
revs = int(numSteps / self.getStepsPerRevolution())
for _ in range(revs):
if not self.isRunning() :
break # Request to terminate early
self.motor.step(int(self.getStepsPerRevolution()), direction, stepType)
self.setCurrentstep(self.getCurrentstep() + self.getStepsPerRevolution() * sign)
remainingSteps = numSteps - (revs * self.getStepsPerRevolution())
if self.isRunning() :
self.motor.step(int(remainingSteps), direction, stepType)
self.setCurrentstep(self.getCurrentstep() + remainingSteps * sign)
self.setIsRunning(False)
return self.getCurrentstep()
def oneStep(self, direction, style):
return self.motor.oneStep(direction, style)
def step(self, steps, direction, style, time_ms, updateSteps=20):
self.__executeSteps(steps, style, direction, time_ms, updateSteps)
def turnOffMotor(self):
self.motor.run(Adafruit_MotorHAT.RELEASE)
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 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 initializeHardware(self):
self.mh = Adafruit_MotorHAT()
self.myStepper = self.mh.getStepper(200,1)
self.myStepper.setSpeed(200)
GPIO.setwarnings(False)
GPIO.cleanup()
GPIO.setmode(GPIO.BCM)
GPIO.setup(self.pin_irsensor_in, GPIO.IN)
GPIO.setup(self.pin_buzzer_out, GPIO.OUT)
GPIO.output(self.pin_buzzer_out, GPIO.LOW)
GPIO.setwarnings(True)
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)
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.leftRPM = 0.0
self.rightRPM = 0.0
self.updatePWM()
class Move():
def __init__(self):
# create a default object, no changes to I2C address or frequency
self.mh = Adafruit_MotorHAT(addr=0x60)
#self.turnOfFMotors = turnOffOmotor()
#atexit.register(self.turnOffMotors())
self.myMotorEngine = self.mh.getMotor(1)
self.myMotorWheel = self.mh.getMotor(4)
self.myMotorEngine.run(Adafruit_MotorHAT.RELEASE)
self.myMotorWheel.run(Adafruit_MotorHAT.RELEASE)
def forward(self):
#forward and then backword
self.myMotorEngine.setSpeed(50)
self.myMotorEngine.run(Adafruit_MotorHAT.FORWARD)
def backwoard(self):
#time.sleep(3)
self.myMotorEngine.setSpeed(50)
self.myMotorEngine.run(Adafruit_MotorHAT.BACKWARD)
def turnleft(self):
# turn left
self.myMotorWheel.setSpeed(150)
self.myMotorWheel.run(Adafruit_MotorHAT.FORWARD)
self.myMotorEngine.run(Adafruit_MotorHAT.FORWARD)
def turnleft(self):
# turn left
self.myMotorWheel.setSpeed(150)
self.myMotorWheel.run(Adafruit_MotorHAT.BACKWARD)
self.myMotorEngine.run(Adafruit_MotorHAT.BACKWARD)
def release(self):
self.myMotorEngine.run(Adafruit_MotorHAT.RELEASE)
self.myMotorWheel.run(Adafruit_MotorHAT.RELEASE)
def __init__(self,
motor,
encoder_reader,
num_samples=10000,
period_ms=1,
message_type=3):
threading.Thread.__init__(self)
# Save inputs
self.motor_number = motor
self.reader = encoder_reader
self.num_samples = num_samples
self.period_ms = period_ms
self.period = datetime.timedelta(seconds=self.period_ms / 1000)
self.message_type = message_type
# Create default object to control the motor using the MototrHAT (I2C)
self.mh = Adafruit_MotorHAT(addr=0x60)
# Create motor variable
self.motor = self.mh.getMotor(self.motor_number)
# Create motor values (vector of values that will be sent to motor)
self.create_motor_values(length_values=self.num_samples)
# Create variable to store motor and encoder data
self.json_list = []
self.json_idx = 0
self.message_number = 0
# Create variables to move motor more efficiently
self.prev_direction = None
self.prev_speed = None
# Create variable to stop thread
self.stop_event = threading.Event()
def __init__(self, addr=0x60, i2c_bus = 1, left_motor_channel = 1, right_motor_channel=2, left_alpha = 1, left_beta = 0,right_alpha=1, right_beta = 0):
print("init jetbot car addr: ", addr, "bus: ", i2c_bus)
if i2c_bus is not None:
from Adafruit_GPIO import I2C
#replace the get_bus function with our own
def get_bus():
return i2c_bus
I2C.get_default_bus = get_bus
self.motor_driver = Adafruit_MotorHAT(addr, i2c_bus)
self.left_alpha = left_alpha
self.left_beta = left_beta
self.right_alpha = right_alpha
self.right_beta = right_beta
self.left_motor = Motor(self.motor_driver, channel=left_motor_channel, alpha=left_alpha, beta = left_beta )
self.right_motor = Motor(self.motor_driver, channel= right_motor_channel, alpha=right_alpha, beta = right_beta )
def __init__(self, *args, **kwargs):
super(Robot, self).__init__(*args, **kwargs)
self.motor_driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
self.left_motor = Motor(self.motor_driver,
channel=self.left_motor_channel,
alpha=self.left_motor_alpha)
self.left_motor2 = Motor(self.motor_driver,
channel=self.left_motor_channel2,
alpha=self.left_motor_alpha2)
self.right_motor = Motor(self.motor_driver,
channel=self.right_motor_channel,
alpha=self.right_motor_alpha)
self.right_motor2 = Motor(self.motor_driver,
channel=self.right_motor_channel2,
alpha=self.right_motor_alpha2)
def __init__(self, motor_gain, wheel_sep, wheel_radius):
self.test_mode = rospy.get_param("~test_mode", False)
self._wheel_sep = wheel_sep
self._wheel_rad = wheel_radius
self._gear_ratio = 7.5 * motor_gain
self._max_rpm = 130
self._max_pwm = 255
self._mh = Adafruit_MotorHAT(addr=0x60)
self._motor_left_num = 1
self._motor_right_num = 2
self._motor_left = self._mh.getMotor(self._motor_left_num)
self._motor_right = self._mh.getMotor(self._motor_right_num)
self._motor_left.setSpeed(0)
self._motor_right.setSpeed(0)
self._motor_left.run(Adafruit_MotorHAT.FORWARD)
self._motor_right.run(Adafruit_MotorHAT.FORWARD)
self._motor_left.run(Adafruit_MotorHAT.RELEASE)
self._motor_right.run(Adafruit_MotorHAT.RELEASE)
self.last_msg_time = None
self.motors_on = False
rospy.on_shutdown(self.turnOffMotors)
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("/kaku/car_cmd", Point, self.cbControl_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)
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!"})
class Adafruit_DCMotor_Hat:
__single = None
def __new__(clz):
if not Adafruit_DCMotor_Hat.__single:
Adafruit_DCMotor_Hat.__single = object.__new__(clz)
return Adafruit_DCMotor_Hat.__single
'''
Adafruit DC Motor Controller
Used for each motor on a differential drive car.
'''
def __init__(self, metaclass=Singleton):
from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor
import atexit
self.FORWARD = Adafruit_MotorHAT.FORWARD
self.BACKWARD = Adafruit_MotorHAT.BACKWARD
self.RELEASE = Adafruit_MotorHAT.RELEASE
self.mh = Adafruit_MotorHAT(addr=0x60)
#self.mh = Adafruit_MotorHAT(addr=0x6f)
self.mhM1 = self.mh.getMotor(1)
self.mhM2 = self.mh.getMotor(2)
def run(self, speed):
'''
Update the speed of the motor where 1 is full forward and
-1 is full backwards.
'''
self.motor.setSpeed(self.throttle)
self.throttle = int(dk.utils.map_range(abs(speed), -1, 1, -255, 255))
def set_pulse(self, pulse):
self.pulse = pulse
###
def set_speed(self, speed):
if self.pulse > 0:
self.mhM2.run(speed)
elif self.pulse < 0:
self.mhM1.run(speed)
def shutdown(self):
self.mhM1.run(self.RELEASE)
self.mhM2.run(self.RELEASE)
def __init__(self, addr=0x60, steering_id=1, drive_id=4):
# Initialize HAT and motors
self.mh = HAT(addr)
self.steering = self.mh.getMotor(steering_id)
self.motor = self.mh.getMotor(drive_id)
self.MAX_SPEED = 255
# Initialize sensors
# Defaults to decision made at 30 cm
GPIO.setmode(GPIO.BCM)
self.usm = Sensor(12, 13) # Middle sensor
self.usr = Sensor(5, 6) # Right side sensor
self.usl = Sensor(8, 7) # Left side sensor
self.usTriggered = None
self.sensors = [self.usl, self.usm, self.usr]
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.")
def __init__(self,
addr=0x60,
left_front_id=1,
left_rear_id=2,
right_front_id=3,
right_rear_id=4):
from Adafruit_MotorHAT import Adafruit_MotorHAT
import atexit
self.mh = Adafruit_MotorHAT(addr)
self.lf = self.MotorWithMotorHat(self.mh, left_front_id)
self.lr = self.MotorWithMotorHat(self.mh, left_rear_id)
self.rf = self.MotorWithMotorHat(self.mh, right_front_id)
self.rr = self.MotorWithMotorHat(self.mh, right_rear_id)
self.stop_motors()
# call of the shutdown method does not seem to be reliable, thus
# register an own exit handler
atexit.register(self.stop_motors)
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)
def __init__(self):
self.hats = []
self.maps = {}
GPIO.setmode(GPIO.BCM)
# Detect plugged hats
for hat in list(MotionController.hats):
try:
self.hats.append(Adafruit_MotorHAT(hat))
except:
MotionController.hats.remove(hat)
print(
'An error occured while processing Adafruit_MotorHAT at I2C address '
+ str(hat))
for index, motor_hat in enumerate(self.hats):
try:
for port in range(2):
motor = motor_hat.getStepper(200, port + 1)
motor.setSpeed(30)
switch = MotionController.switches[index * 2 + port]
GPIO.setup(switch, GPIO.IN)
start = time.time()
elapse = time.time() - start
while elapse < MotionController.MAX_TIME and GPIO.input(
switch) == 0:
motor.step(5, Adafruit_MotorHAT.BACKWARD,
Adafruit_MotorHAT.DOUBLE)
elapse = time.time() - start
if elapse < MotionController.MAX_TIME:
print('Index ', index, 'Switch ', switch,
' answered to motor ', hat, ':', port)
self.maps[self.addresses[index * 2 + port]] = [
switch, motor_hat, port + 1
]
else:
print('Index ', index, 'Switch ', switch,
' did not answered to motor ', hat, ':', port)
motor_hat.getMotor(port + 1).run(Adafruit_MotorHAT.RELEASE)
except:
print(
'An error occured while processing a port of an Adafruit_MotorHAT'
)
self.positions = [0 for x in range(len(self.maps))]
print(self.maps)
class Motor():
def __init__(self, channel, alpha=0.5, offset=0.2, i2c_bus=1, debug=False):
self.i2c_bus, self.alpha, self.offset = i2c_bus, alpha, offset
self.driver = Adafruit_MotorHAT(i2c_bus=self.i2c_bus)
self.motor = self.driver.getMotor(channel)
self._debug = debug
self._channel = channel
self._old_speed = -1
if (channel == 1):
self.ina, self.inb = 1, 0
else:
self.ina, self.inb = 2, 3
atexit.register(self.stop)
def drive(self, value):
# sets motor value between [-1, 1]
if value > 0.0:
mapped_value = int(255.0 * (self.alpha * value + self.offset))
elif value < 0.0:
mapped_value = int(255.0 * (self.alpha * value - self.offset))
else:
mapped_value = 0
speed = min(max(abs(mapped_value), 0), 255)
# debug : write changed speed
if self._debug and self._old_speed != speed:
print(f'channel {self._channel} speed:{speed}')
self._old_speed = speed
self.motor.setSpeed(speed)
if mapped_value < 0:
self.motor.run(Adafruit_MotorHAT.FORWARD)
self.driver._pwm.setPWM(self.ina, 0, 0)
self.driver._pwm.setPWM(self.inb, 0, speed * 16)
else:
self.motor.run(Adafruit_MotorHAT.BACKWARD)
self.driver._pwm.setPWM(self.ina, 0, speed * 16)
self.driver._pwm.setPWM(self.inb, 0, 0)
def stop(self):
self.motor.run(Adafruit_MotorHAT.RELEASE)
self.driver._pwm.setPWM(self.ina, 0, 0)
self.driver._pwm.setPWM(self.inb, 0, 0)
def __init__(self,mh_addr=0x60):
#使用给定的地址设置motorHAT
self._mh=MotorHAT(addr=mh_addr)
#设置两个马达
self.left_motor=self._mh.getMotor(1)
self.right_motor=self._mh.getMotor(2)
# recommended for auto-disabling motors on shutdown!
atexit.register(self.stop_all)
#设置两个巡线传感器
self.left_line_sensor=LineSensor(23)
self.right_line_sensor=LineSensor(16)
#设置全彩LED
self.leds=neopixel.NeoPixel(pixel_pin, num_pixels, brightness=0.2, auto_write=False,
pixel_order=ORDER)
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 LinearActuator:
def __init__(self,
hat_addr,
channels,
freq=default_freq,
name="",
speed=255):
self._motor_hat = Adafruit_MotorHAT(addr=hat_addr)
self._channels = []
self.name = name
for chan in channels:
c = self._motor_hat.getMotor(chan)
# motor will not run unless speed is explicitly set
c.setSpeed(speed) # 0 to 255
self._channels.append(c)
def stopActuator(self):
if debug:
print("Stopping {}\n".format(self.name))
for chan in self._channels:
chan.run(Adafruit_MotorHAT.RELEASE)
def _startExtend(self):
for chan in self._channels:
chan.run(Adafruit_MotorHAT.FORWARD)
def _startRetract(self):
for chan in self._channels:
chan.run(Adafruit_MotorHAT.BACKWARD)
def extend(self):
if debug:
print("Moving {} up\n\t".format(self.name))
self._startExtend()
sleep(extend_time_sec)
self.stopActuator()
def retract(self):
if debug:
print("Moving {} down\n\t".format(self.name))
self._startRetract()
sleep(retract_time_sec)
self.stopActuator()
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)
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 > 100:
u_R = 100
if u_L > 100:
u_L = 100
if u_R > self.threshold:
self.gazebo_car_control_R.setSpeed(u_R)
self.gazebo_car_control_R.run(Adafruit_MotorHAT.FORWARD)
if u_R < -(self.threshold):
self.gazebo_car_control_R.setSpeed(-u_R)
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)
if u_L < -(self.threshold):
self.gazebo_car_control_L.setSpeed(-u_L)
self.gazebo_car_control_L.run(Adafruit_MotorHAT.BACKWARD)
# self.gazebo_car_control_.run(Adafruit_MotorHAT.RELEASE)
def onShutdown(self):
rospy.loginfo("[gazebo_car_control_node] Shutdown.")
def __init__(self, addr=0x60, left_one_id=1, left_two_id=2, right_one_id=3, right_two_id=4):
# Setup ROS node
self.node_name = rospy.get_name()
# Setup motor name
self._mh = Adafruit_MotorHAT(addr)
self._leftOne = self.mh.getMotor(left_one_id)
self._leftTwo = self.mh.getMotor(left_two_id)
self._rightOne = self.mh.getMotor(right_one_id)
self._rightTwo = self.mh.getMotor(right_two_id)
# Start with motor turn off
self._leftOne.run(Adafruit_MotorHAT.RELEASE)
self._leftTwo.run(Adafruit_MotorHAT.RELEASE)
self._rightOne.run(Adafruit_MotorHAT.RELEASE)
self._rightTwo.run(Adafruit_MotorHAT.RELEASE)
# Setup the publisher and subscriber
self.sub_slideway_cmd = rospy.Subscriber("~slideway_cmd", )
def __init__(self):
"""
Instantiates arrays and encoder objects
"""
mh = mh = Adafruit_MotorHAT(addr=0x60)
e = [None, None, None, None]
for motor in xrange(0, 4):
# Init encoders
ePin = PINS[motor]
if ePin is not None:
e[motor] = Encoder(ePin)
else:
e[motor] = Encoder(-1)
# Set GPIO pins for writing implement
GPIO.setmode(GPIO.BCM)
GPIO.setup(WRITINGPINS[0], GPIO.OUT)
GPIO.setup(WRITINGPINS[1], GPIO.OUT)
GPIO.setup(WRITINGPINS[2], GPIO.OUT)
self.pwm = GPIO.PWM(WRITINGPINS[2], 490)
self.pwm.start(0)
self.encoders = e
self.prevErrors = np.array([0.0, 0.0, 0.0, 0.0])
# Thread exit flags
self.stopFlag = False
self.currThread = None
self.motors = [
mh.getMotor(1),
mh.getMotor(2),
mh.getMotor(3),
mh.getMotor(4)
]
atexit.register(self.stopMotors)
# Writing implement starts off unactuated
self.isWriting = False
self.writingThread = None
self._debug = 1
def __init__(self):
#logger.info("init move")
# setup motor controller
self.motor_left_ID = 1
self.motor_right_ID = 2
if MOTOR_CONTROLLER == 'adafruit':
self.motor_driver = Adafruit_MotorHAT(
i2c_bus=int(rospy.get_param("i2c_bus")))
# self.motor_left_ID = 1
# self.motor_right_ID = 2
self.motor_left = self.motor_driver.getMotor(self.motor_left_ID)
self.motor_right = self.motor_driver.getMotor(self.motor_right_ID)
self.all_stop()
elif MOTOR_CONTROLLER == 'qwiic':
logger.info("init move - qwiic")
self.motor_driver = QwiicScmd()
logger.info("init move - qwiic send commd")
if self.motor_driver.connected == False:
logger.info("init move - conneectedd")
else:
logger.info("init move - conneectedd")
class DoorMotor():
def __init__(self, motorNumber=0):
self.motorNumber = motorNumber
self.mh = Adafruit_MotorHAT(0x70)
self.m = self.mh.getMotor(1)
def openMotorDoor(self):
self.m.run(Adafruit_MotorHAT.BACKWARD)
for i in range(100):
self.m.setSpeed(i)
time.sleep(.1)
time.sleep(5)
self.m.run(Adafruit_MotorHAT.RELEASE)
def closeMotorDoor(self):
self.m.run(Adafruit_MotorHAT.FORWARD)
for i in range(100):
self.m.setSpeed(i)
time.sleep(.1)
time.sleep(5)
self.m.run(Adafruit_MotorHAT.RELEASE)
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 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))
def init(cArgs, forwardDefinition, leftDefinition, pEnabled):
global mh
global commandArgs
global drivingSpeed
global speed1
global left
global right
global forward
global backward
global motorA
global motorB
global pingPongEnabled
global mhPingPong
pingPongEnabled = pEnabled
forward = forwardDefinition
backward = times(forward, -1)
left = leftDefinition
right = times(left, -1)
print("directions", forward, backward, left, right)
commandArgs = cArgs
drivingSpeed = commandArgs.straight_speed
speed1 = drivingSpeed
_thread.start_new_thread(demoShots, ())
atexit.register(turnOffMotors)
motorA = mh.getMotor(1)
motorB = mh.getMotor(2)
atexit.register(turnOffMotors)
if pingPongEnabled:
mhPingPong = Adafruit_MotorHAT(addr=0x61)
class RobieLegs:
mh = Adafruit_MotorHAT(addr=0x60)
leftLeg = mh.getMotor(2)
rightLeg = mh.getMotor(1)
def turnOffMotors(self):
self.rightLeg.run(Adafruit_MotorHAT.RELEASE)
self.leftLeg.run(Adafruit_MotorHAT.RELEASE)
def roll(self, x, y):
if y >= 0:
self.leftLeg.run(Adafruit_MotorHAT.FORWARD)
self.rightLeg.run(Adafruit_MotorHAT.FORWARD)
elif y < 0:
y = y * -1
self.leftLeg.run(Adafruit_MotorHAT.BACKWARD)
self.rightLeg.run(Adafruit_MotorHAT.BACKWARD)
if x < 0:
x = x * -1
leftSpeed = y
rightSpeed = y - x
if rightSpeed < 0:
rightSpeed = rightSpeed * -1
elif x >= 0:
rightSpeed = y
leftSpeed = y - x
if leftSpeed < 0:
leftSpeed = leftSpeed * -1
self.rightLeg.setSpeed(rightSpeed)
self.leftLeg.setSpeed(leftSpeed)
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 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.pwm = PWM(address=0x40, debug=False)
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
self.pwm.setPWM(0, 4095, 4095)
self.pwm.setPWM(1, 4095, 4095)
self.pwm.setPWM(2, 4095, 4095)
self.pwm.setPWM(3, 4095, 4095)
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
self.pwm.setPWM(0, 4095, 4095)
self.pwm.setPWM(1, 4095, 4095)
self.pwm.setPWM(2, 4095, 4095)
self.pwm.setPWM(3, 4095, 4095)
elif vr > 0:
rightMotorMode = Adafruit_MotorHAT.FORWARD
elif vr < 0:
rightMotorMode = Adafruit_MotorHAT.BACKWARD
if vl > 0 and vr < 0:
self.pwm.setPWM(2, 0, 4095)
if vl < 0 and vr > 0:
self.pwm.setPWM(3, 0, 4095)
if vl > 0 and vr > 0:
self.pwm.setPWM(0, 0, 4095)
if vl < 0 and vr < 0:
self.pwm.setPWM(1, 0, 4095)
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
