diff --git a/dw640HAT/dw640HAT.py b/dw640HAT/dw640HAT.py index 43278fe..11ad8cf 100644 --- a/dw640HAT/dw640HAT.py +++ b/dw640HAT/dw640HAT.py @@ -1,5 +1,6 @@ #!/usr/bin/python +import RPi.GPIO as GPIO from Adafruit_PWM_Servo_Driver import PWM import time @@ -169,42 +170,75 @@ class dw_DCMotor: def __init__(self, controller, num): self.MC = controller self.motornum = num - pwm = in1 = in2 = 0 + modepin = in1 = in2 = 0 if (num == 0): - pwm = 8 - in2 = 9 - in1 = 10 + modepin = 11 + in2 = 8 + in1 = 9 elif (num == 1): - pwm = 13 - in2 = 12 + modepin = 11 + in2 = 10 in1 = 11 elif (num == 2): - pwm = 2 - in2 = 3 - in1 = 4 + modepin = 13 + in2 = 7 + in1 = 6 elif (num == 3): - pwm = 7 - in2 = 6 - in1 = 5 + modepin = 13 + in2 = 5 + in1 = 4 + elif (num == 4): + modepin = 15 + in2 = 0 + in1 = 1 + elif (num == 5): + modepin = 15 + in2 = 2 + in1 = 3 + else: - raise NameError('MotorHAT Motor must be between 1 and 4 inclusive') - self.PWMpin = pwm + raise NameError('MotorHAT Motor must be between 1 and 6 inclusive') + self.MODEpin = modepin self.IN1pin = in1 self.IN2pin = in2 + + + def setMode(self, level): + if not self.MC: + return + if (level == dw_MotorCONTROL.ININ): + GPIO.output(self.MODEpin, GPIO.LOW) + + if (level == dw_MotorCONTROL.PHASE): + GPIO.output(self.MODEpin, GPIO.HIGH) + + def run(self, command): if not self.MC: return - if (command == dw_MotorCONTROL.FORWARD): - self.MC.setPin(self.IN2pin, 0) - self.MC.setPin(self.IN1pin, 1) - if (command == dw_MotorCONTROL.BACKWARD): - self.MC.setPin(self.IN1pin, 0) - self.MC.setPin(self.IN2pin, 1) - if (command == dw_MotorCONTROL.RELEASE): - self.MC.setPin(self.IN1pin, 0) - self.MC.setPin(self.IN2pin, 0) + if( GPIO.input( self.MODEpin ) == GPIO.LOW ): + if (command == dw_MotorCONTROL.FORWARD): + self.MC.setPin(self.IN2pin, 0) + self.MC.setPin(self.IN1pin, 1) + if (command == dw_MotorCONTROL.BACKWARD): + self.MC.setPin(self.IN1pin, 0) + self.MC.setPin(self.IN2pin, 1) + if (command == dw_MotorCONTROL.RELEASE): + self.MC.setPin(self.IN1pin, 0) + self.MC.setPin(self.IN2pin, 0) + else: + if (command == dw_MotorCONTROL.FORWARD): + self.MC.setPin(self.IN2pin, 0) + self.MC.setPin(self.IN1pin, 1) + if (command == dw_MotorCONTROL.BACKWARD): + self.MC.setPin(self.IN1pin, 0) + self.MC.setPin(self.IN2pin, 1) + if (command == dw_MotorCONTROL.RELEASE): + self.MC.setPin(self.IN1pin, 0) + self.MC.setPin(self.IN2pin, 0) + def setSpeed(self, speed): if (speed < 0): speed = 0 @@ -223,14 +257,26 @@ class dw_MotorCONTROL: INTERLEAVE = 3 MICROSTEP = 4 + ININ = 0 + PHASE = 1 + def __init__(self, addr = 0x60, freq = 1600): self._i2caddr = addr # default addr on HAT self._frequency = freq # default @1600Hz PWM freq - self.motors = [ dw_DCMotor(self, m) for m in range(4) ] + self.motors = [ dw_DCMotor(self, m) for m in range(6) ] # self.steppers = [ Adafruit_StepperMotor(self, 1), Adafruit_StepperMotor(self, 2) ] self._pwm = PWM(addr, debug=False) self._pwm.setPWMFreq(self._frequency) + GPIO.setmode(GPIO.BCM) + GPIO.setup(11, GPIO.OUT) + GPIO.setup(13, GPIO.OUT) + GPIO.setup(15, GPIO.OUT) + + GPIO.output(11, GPIO.LOW) + GPIO.output(13, GPIO.LOW) + GPIO.output(15, GPIO.LOW) + def setPin(self, pin, value): if (pin < 0) or (pin > 15): raise NameError('PWM pin must be between 0 and 15 inclusive') @@ -247,6 +293,6 @@ class dw_MotorCONTROL: # return self.steppers[num-1] def getMotor(self, num): - if (num < 1) or (num > 4): - raise NameError('MotorHAT Motor must be between 1 and 4 inclusive') + if (num < 1) or (num > 6): + raise NameError('MotorHAT Motor must be between 1 and 6 inclusive') return self.motors[num-1] diff --git a/examples/DCTest.py b/examples/DCTest.py deleted file mode 100644 index a41f7ea..0000000 --- a/examples/DCTest.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/python -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor - -import time -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) - time.sleep(0.01) - - print "\tSlow down..." - for i in reversed(range(255)): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "Backward! " - myMotor.run(Adafruit_MotorHAT.BACKWARD) - - print "\tSpeed up..." - for i in range(255): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "\tSlow down..." - for i in reversed(range(255)): - myMotor.setSpeed(i) - time.sleep(0.01) - - print "Release" - myMotor.run(Adafruit_MotorHAT.RELEASE) - time.sleep(1.0) diff --git a/examples/DualStepperTest.py b/examples/DualStepperTest.py deleted file mode 100644 index 8a8ea08..0000000 --- a/examples/DualStepperTest.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/python -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor -import time -import atexit -import threading -import random - -# create a default object, no changes to I2C address or frequency -mh = Adafruit_MotorHAT() - -# create empty threads (these will hold the stepper 1 and 2 threads) -st1 = threading.Thread() -st2 = threading.Thread() - - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - -atexit.register(turnOffMotors) - -myStepper1 = mh.getStepper(200, 1) # 200 steps/rev, motor port #1 -myStepper2 = mh.getStepper(200, 2) # 200 steps/rev, motor port #1 -myStepper1.setSpeed(60) # 30 RPM -myStepper2.setSpeed(60) # 30 RPM - - -stepstyles = [Adafruit_MotorHAT.SINGLE, Adafruit_MotorHAT.DOUBLE, Adafruit_MotorHAT.INTERLEAVE, Adafruit_MotorHAT.MICROSTEP] - -def stepper_worker(stepper, numsteps, direction, style): - #print("Steppin!") - stepper.step(numsteps, direction, style) - #print("Done") - -while (True): - if not st1.isAlive(): - randomdir = random.randint(0, 1) - print("Stepper 1"), - if (randomdir == 0): - dir = Adafruit_MotorHAT.FORWARD - print("forward"), - else: - dir = Adafruit_MotorHAT.BACKWARD - print("backward"), - randomsteps = random.randint(10,50) - print("%d steps" % randomsteps) - st1 = threading.Thread(target=stepper_worker, args=(myStepper1, randomsteps, dir, stepstyles[random.randint(0,3)],)) - st1.start() - - if not st2.isAlive(): - print("Stepper 2"), - randomdir = random.randint(0, 1) - if (randomdir == 0): - dir = Adafruit_MotorHAT.FORWARD - print("forward"), - else: - dir = Adafruit_MotorHAT.BACKWARD - print("backward"), - - randomsteps = random.randint(10,50) - print("%d steps" % randomsteps) - - st2 = threading.Thread(target=stepper_worker, args=(myStepper2, randomsteps, dir, stepstyles[random.randint(0,3)],)) - st2.start() diff --git a/examples/Robot.py b/examples/Robot.py deleted file mode 100644 index 1d61d04..0000000 --- a/examples/Robot.py +++ /dev/null @@ -1,120 +0,0 @@ -# Simple two DC motor robot class. Exposes a simple LOGO turtle-like API for -# moving a robot forward, backward, and turning. See RobotTest.py for an -# example of using this class. -# Author: Tony DiCola -# License: MIT License https://opensource.org/licenses/MIT -import time -import atexit - -from Adafruit_MotorHAT import Adafruit_MotorHAT - - -class Robot(object): - def __init__(self, addr=0x60, left_id=1, right_id=2, 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 _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 forward(self, speed, seconds=None): - """Move forward at the specified speed (0-255). Will start moving - forward and return unless a seconds value is specified, in which - case the robot will move forward for that amount of time and then stop. - """ - # Set motor speed and move both forward. - self._left_speed(speed) - self._right_speed(speed) - self._left.run(Adafruit_MotorHAT.FORWARD) - self._right.run(Adafruit_MotorHAT.FORWARD) - # If an amount of time is specified, move for that time and then stop. - if seconds is not None: - time.sleep(seconds) - self.stop() - - def backward(self, speed, seconds=None): - """Move backward at the specified speed (0-255). Will start moving - backward and return unless a seconds value is specified, in which - case the robot will move backward for that amount of time and then stop. - """ - # Set motor speed and move both backward. - self._left_speed(speed) - self._right_speed(speed) - self._left.run(Adafruit_MotorHAT.BACKWARD) - self._right.run(Adafruit_MotorHAT.BACKWARD) - # If an amount of time is specified, move for that time and then stop. - if seconds is not None: - time.sleep(seconds) - self.stop() - - def right(self, speed, seconds=None): - """Spin to the right at the specified speed. Will start spinning and - return unless a seconds value is specified, in which case the robot will - spin for that amount of time and then stop. - """ - # Set motor speed and move both forward. - self._left_speed(speed) - self._right_speed(speed) - self._left.run(Adafruit_MotorHAT.FORWARD) - self._right.run(Adafruit_MotorHAT.BACKWARD) - # If an amount of time is specified, move for that time and then stop. - if seconds is not None: - time.sleep(seconds) - self.stop() - - def left(self, speed, seconds=None): - """Spin to the left at the specified speed. Will start spinning and - return unless a seconds value is specified, in which case the robot will - spin for that amount of time and then stop. - """ - # Set motor speed and move both forward. - self._left_speed(speed) - self._right_speed(speed) - self._left.run(Adafruit_MotorHAT.BACKWARD) - self._right.run(Adafruit_MotorHAT.FORWARD) - # If an amount of time is specified, move for that time and then stop. - if seconds is not None: - time.sleep(seconds) - self.stop() diff --git a/examples/RobotTest.py b/examples/RobotTest.py deleted file mode 100644 index 86454bb..0000000 --- a/examples/RobotTest.py +++ /dev/null @@ -1,65 +0,0 @@ -# Simple two DC motor robot class usage example. -# Author: Tony DiCola -# License: MIT License https://opensource.org/licenses/MIT -import time - -# Import the Robot.py file (must be in the same directory as this file!). -import Robot - - -# Set the trim offset for each motor (left and right). This is a value that -# will offset the speed of movement of each motor in order to make them both -# move at the same desired speed. Because there's no feedback the robot doesn't -# know how fast each motor is spinning and the robot can pull to a side if one -# motor spins faster than the other motor. To determine the trim values move the -# robot forward slowly (around 100 speed) and watch if it veers to the left or -# right. If it veers left then the _right_ motor is spinning faster so try -# setting RIGHT_TRIM to a small negative value, like -5, to slow down the right -# motor. Likewise if it veers right then adjust the _left_ motor trim to a small -# negative value. Increase or decrease the trim value until the bot moves -# straight forward/backward. -LEFT_TRIM = 0 -RIGHT_TRIM = 0 - - -# Create an instance of the robot with the specified trim values. -# Not shown are other 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. -robot = Robot.Robot(left_trim=LEFT_TRIM, right_trim=RIGHT_TRIM) - -# Now move the robot around! -# Each call below takes two parameters: -# - speed: The speed of the movement, a value from 0-255. The higher the value -# the faster the movement. You need to start with a value around 100 -# to get enough torque to move the robot. -# - time (seconds): Amount of time to perform the movement. After moving for -# this amount of seconds the robot will stop. This parameter -# is optional and if not specified the robot will start moving -# forever. -robot.forward(150, 1.0) # Move forward at speed 150 for 1 second. -robot.left(200, 0.5) # Spin left at speed 200 for 0.5 seconds. -robot.forward(150, 1.0) # Repeat the same movement 3 times below... -robot.left(200, 0.5) -robot.forward(150, 1.0) -robot.left(200, 0.5) -robot.forward(150, 1.0) -robot.right(200, 0.5) - -# Spin in place slowly for a few seconds. -robot.right(100) # No time is specified so the robot will start spinning forever. -time.sleep(2.0) # Pause for a few seconds while the robot spins (you could do - # other processing here though!). -robot.stop() # Stop the robot from moving. - -# Now move backwards and spin right a few times. -robot.backward(150, 1.0) -robot.right(200, 0.5) -robot.backward(150, 1.0) -robot.right(200, 0.5) -robot.backward(150, 1.0) -robot.right(200, 0.5) -robot.backward(150, 1.0) - -# That's it! Note that on exit the robot will automatically stop moving. diff --git a/examples/StackingTest.py b/examples/StackingTest.py deleted file mode 100644 index 9a0d016..0000000 --- a/examples/StackingTest.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/python -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor -import time -import atexit -import threading -import random - -# bottom hat is default address 0x60 -bottomhat = Adafruit_MotorHAT(addr=0x60) -# top hat has A0 jumper closed, so its address 0x61 -tophat = Adafruit_MotorHAT(addr=0x61) - -# create empty threads (these will hold the stepper 1, 2 & 3 threads) -stepperThreads = [threading.Thread(), threading.Thread(), threading.Thread()] - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - tophat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - tophat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - tophat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - tophat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - bottomhat.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - bottomhat.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - bottomhat.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - bottomhat.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - -atexit.register(turnOffMotors) - -myStepper1 = bottomhat.getStepper(200, 1) # 200 steps/rev, motor port #1 -myStepper2 = bottomhat.getStepper(200, 2) # 200 steps/rev, motor port #2 -myStepper3 = tophat.getStepper(200, 1) # 200 steps/rev, motor port #1 - -myStepper1.setSpeed(60) # 60 RPM -myStepper2.setSpeed(30) # 30 RPM -myStepper3.setSpeed(15) # 15 RPM - -# get a DC motor! -myMotor = tophat.getMotor(3) -# set the speed to start, from 0 (off) to 255 (max speed) -myMotor.setSpeed(150) -# turn on motor -myMotor.run(Adafruit_MotorHAT.FORWARD); - - -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): - for i in range(3): - if not stepperThreads[i].isAlive(): - randomdir = random.randint(0, 1) - print("Stepper %d" % i), - if (randomdir == 0): - dir = Adafruit_MotorHAT.FORWARD - print("forward"), - else: - dir = Adafruit_MotorHAT.BACKWARD - print("backward"), - randomsteps = random.randint(10,50) - print("%d steps" % randomsteps) - stepperThreads[i] = threading.Thread(target=stepper_worker, args=(steppers[i], randomsteps, dir, stepstyles[random.randint(0,len(stepstyles)-1)],)) - stepperThreads[i].start() - - # also, lets switch around the DC motor! - myMotor.setSpeed(random.randint(0,255)) # random speed - #myMotor.run(random.randint(0,1)) # random forward/back diff --git a/examples/StepperTest.py b/examples/StepperTest.py deleted file mode 100644 index ba59203..0000000 --- a/examples/StepperTest.py +++ /dev/null @@ -1,38 +0,0 @@ -#!/usr/bin/python -#import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_Stepper -from Adafruit_MotorHAT import Adafruit_MotorHAT, Adafruit_DCMotor, Adafruit_StepperMotor - -import time -import atexit - -# create a default object, no changes to I2C address or frequency -mh = Adafruit_MotorHAT() - -# recommended for auto-disabling motors on shutdown! -def turnOffMotors(): - mh.getMotor(1).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(2).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(3).run(Adafruit_MotorHAT.RELEASE) - mh.getMotor(4).run(Adafruit_MotorHAT.RELEASE) - -atexit.register(turnOffMotors) - -myStepper = mh.getStepper(200, 1) # 200 steps/rev, motor port #1 -myStepper.setSpeed(30) # 30 RPM - -while (True): - print("Single coil steps") - myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.SINGLE) - myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.SINGLE) - - print("Double coil steps") - myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.DOUBLE) - myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.DOUBLE) - - print("Interleaved coil steps") - myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.INTERLEAVE) - myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.INTERLEAVE) - - print("Microsteps") - myStepper.step(100, Adafruit_MotorHAT.FORWARD, Adafruit_MotorHAT.MICROSTEP) - myStepper.step(100, Adafruit_MotorHAT.BACKWARD, Adafruit_MotorHAT.MICROSTEP)