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