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