diff --git a/Adafruit_MotorShield/Adafruit_DCMotor.py b/Adafruit_MotorShield/Adafruit_DCMotor.py new file mode 100644 index 00000000..1edd0808 --- /dev/null +++ b/Adafruit_MotorShield/Adafruit_DCMotor.py @@ -0,0 +1,35 @@ +#!/usr/bin/python + +import time + +class Adafruit_DCMotor : + + FORWARD = 1 + BACKWARD = 2 + BRAKE = 3 + RELEASE = 4 + + LOW = 0 + HIGH = 1 + + def __init__(self, debug=False): + self.debug = debug + self.MC = None + self.motornum = None + self.PWMpin = 0 + self.IN1pin = 0 + self.IN2pin = 0 + + def setSpeed(self, speed): + self.MC.setPWM(self.PWMpin, speed*16); + + def run(self, cmd): + if cmd == self.FORWARD: + self.MC.setPin(self.IN2pin, self.LOW) # take low first to avoid 'break' + self.MC.setPin(self.IN1pin, self.HIGH) + elif cmd == self.BACKWARD: + self.MC.setPin(self.IN1pin, self.LOW) # take low first to avoid 'break' + self.MC.setPin(self.IN2pin, self.HIGH) + elif cmd == self.RELEASE: + self.MC.setPin(self.IN1pin, self.LOW) + self.MC.setPin(self.IN2pin, self.LOW) diff --git a/Adafruit_MotorShield/Adafruit_I2C.py b/Adafruit_MotorShield/Adafruit_I2C.py new file mode 100644 index 00000000..77f06164 --- /dev/null +++ b/Adafruit_MotorShield/Adafruit_I2C.py @@ -0,0 +1 @@ +../Adafruit_I2C/Adafruit_I2C.py \ No newline at end of file diff --git a/Adafruit_MotorShield/Adafruit_MotorShield.py b/Adafruit_MotorShield/Adafruit_MotorShield.py new file mode 100644 index 00000000..04ae54eb --- /dev/null +++ b/Adafruit_MotorShield/Adafruit_MotorShield.py @@ -0,0 +1,100 @@ +#!/usr/bin/python + +from Adafruit_StepperMotor import Adafruit_StepperMotor +from Adafruit_DCMotor import Adafruit_DCMotor +from Adafruit_PWM_Servo_Driver import PWM + +class Adafruit_MotorShield : + + LOW = 0 + HIGHT = 1 + + def __init__(self, address=0x60, debug=False): + self.address = address + self.pwm = PWM(address, debug) + self.steppers = [Adafruit_StepperMotor(debug), Adafruit_StepperMotor(debug)] + self.dcmotors = [Adafruit_DCMotor(debug), Adafruit_DCMotor(debug), Adafruit_DCMotor(debug), Adafruit_DCMotor(debug)] + + def begin(self, freq=1600): + self.pwm.begin() + self.freq = freq + self.pwm.setPWMFreq(freq); # This is the maximum PWM frequency + for i in range(0, 16): + self.pwm.setPWM(i, 0, 0); + + def setPWM(self, pin, value): + if value > 4095: + self.pwm.setPWM(pin, 4096, 0) + else: + self.pwm.setPWM(pin, 0, value); + + def setPin(self, pin, value): + if value == self.LOW: + self.pwm.setPWM(pin, 0, 0) + else: + self.pwm.setPWM(pin, 4096, 0) + + def getMotor(self, num): + if num > 4: + return + + num -= 1 + if (self.dcmotors[num].motornum is None): + # not init'd yet! + self.dcmotors[num].motornum = num; + self.dcmotors[num].MC = self; + if (num == 0): + pwm = 8 + in2 = 9 + in1 = 10 + elif (num == 1): + pwm = 13 + in2 = 12 + in1 = 11 + elif (num == 2): + pwm = 2 + in2 = 3 + in1 = 4 + elif (num == 3): + pwm = 7 + in2 = 6 + in1 = 5 + self.dcmotors[num].PWMpin = pwm; + self.dcmotors[num].IN1pin = in1; + self.dcmotors[num].IN2pin = in2; + + return self.dcmotors[num]; + + def getStepper(self, steps, num): + if num > 2: + return + + num -= 1 + if self.steppers[num].steppernum is None: + # not init'd yet! + self.steppers[num].steppernum = num; + self.steppers[num].revsteps = steps; + self.steppers[num].MC = self + + if num == 0: + pwma = 8 + ain2 = 9 + ain1 = 10 + pwmb = 13 + bin2 = 12 + bin1 = 11 + elif num == 1: + pwma = 2 + ain2 = 3 + ain1 = 4 + pwmb = 7 + bin2 = 6 + bin1 = 5 + self.steppers[num].PWMApin = pwma + self.steppers[num].PWMBpin = pwmb + self.steppers[num].AIN1pin = ain1 + self.steppers[num].AIN2pin = ain2 + self.steppers[num].BIN1pin = bin1 + self.steppers[num].BIN2pin = bin2 + + return self.steppers[num] diff --git a/Adafruit_MotorShield/Adafruit_PWM_Servo_Driver.py b/Adafruit_MotorShield/Adafruit_PWM_Servo_Driver.py new file mode 100644 index 00000000..99674220 --- /dev/null +++ b/Adafruit_MotorShield/Adafruit_PWM_Servo_Driver.py @@ -0,0 +1,81 @@ +#!/usr/bin/python + +import time +import math +from Adafruit_I2C import Adafruit_I2C + +# ============================================================================ +# Adafruit PCA9685 16-Channel PWM Servo Driver +# ============================================================================ + +class PWM : + # Registers/etc. + __MODE1 = 0x00 + __SUBADR1 = 0x02 + __SUBADR2 = 0x03 + __SUBADR3 = 0x04 + __PRESCALE = 0xFE + __LED0_ON_L = 0x06 + __LED0_ON_H = 0x07 + __LED0_OFF_L = 0x08 + __LED0_OFF_H = 0x09 + __ALL_LED_ON_L = 0xFA + __ALL_LED_ON_H = 0xFB + __ALL_LED_OFF_L = 0xFC + __ALL_LED_OFF_H = 0xFD + + # Bits + __RESTART = 0x80 + __SLEEP = 0x10 + __ALLCALL = 0x01 + __INVRT = 0x10 + __OUTDRV = 0x04 + + #general_call_i2c = Adafruit_I2C(0x00) + + def __init__(self, address=0x40, debug=False): + self.i2c = Adafruit_I2C(address) + self.i2c.debug = debug + self.address = address + self.debug = debug + + def begin(self): + self.reset() + + def reset(self): + self.i2c.write8(self.__MODE1, 0x0) + + def setPWMFreq(self, freq): + "Sets the PWM frequency" + prescaleval = 25000000.0 # 25MHz + prescaleval /= 4096.0 # 12-bit + prescaleval /= float(freq) + prescaleval -= 1.0 + if (self.debug): + print "Setting PWM frequency to %d Hz" % freq + print "Estimated pre-scale: %d" % prescaleval + prescale = math.floor(prescaleval + 0.5) + if (self.debug): + print "Final pre-scale: %d" % prescale + + oldmode = self.i2c.readU8(self.__MODE1); + newmode = (oldmode & 0x7F) | 0x10 # sleep + self.i2c.write8(self.__MODE1, newmode) # go to sleep + self.i2c.write8(self.__PRESCALE, int(math.floor(prescale))) + self.i2c.write8(self.__MODE1, oldmode) + time.sleep(0.005) + self.i2c.write8(self.__MODE1, oldmode | 0x80) + + def setPWM(self, channel, on, off): + "Sets a single PWM channel" + self.i2c.write8(self.__LED0_ON_L+4*channel, on & 0xFF) + self.i2c.write8(self.__LED0_ON_H+4*channel, on >> 8) + self.i2c.write8(self.__LED0_OFF_L+4*channel, off & 0xFF) + self.i2c.write8(self.__LED0_OFF_H+4*channel, off >> 8) + + def setAllPWM(self, on, off): + "Sets a all PWM channels" + self.i2c.write8(self.__ALL_LED_ON_L, on & 0xFF) + self.i2c.write8(self.__ALL_LED_ON_H, on >> 8) + self.i2c.write8(self.__ALL_LED_OFF_L, off & 0xFF) + self.i2c.write8(self.__ALL_LED_OFF_H, off >> 8) diff --git a/Adafruit_MotorShield/Adafruit_StepperMotor.py b/Adafruit_MotorShield/Adafruit_StepperMotor.py new file mode 100644 index 00000000..7c5ef38c --- /dev/null +++ b/Adafruit_MotorShield/Adafruit_StepperMotor.py @@ -0,0 +1,208 @@ +#!/usr/bin/python + +import time + +class Adafruit_StepperMotor : + + __MICROSTEPCURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255] + #__MICROSTEPCURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255] #with MICROSTEPS = 8 + MICROSTEPS = 16 #8 or 16 + + FORWARD = 1 + BACKWARD = 2 + BRAKE = 3 + RELEASE = 4 + + SINGLE = 1 + DOUBLE = 2 + INTERLEAVE = 3 + MICROSTEP = 4 + + LOW = 0 + HIGH = 1 + + def __init__(self, debug=False): + self.debug = debug + self.revsteps = 0 + self.steppernum = None + self.currentstep = 0 + + def setSpeed(self, rpm): + #Serial.println("steps per rev: "); Serial.println(revsteps); + #Serial.println("RPM: "); Serial.println(rpm); + + self.usperstep = 60000000.0 / (self.revsteps * rpm) + self.steppingcounter = 0 + + def release(self): + self.MC.setPin(self.AIN1pin, self.LOW) + self.MC.setPin(self.AIN2pin, self.LOW) + self.MC.setPin(self.BIN1pin, self.LOW) + self.MC.setPin(self.BIN2pin, self.LOW) + self.MC.setPWM(self.PWMApin, 0) + self.MC.setPWM(self.PWMBpin, 0) + + def step(self, steps, dir, style): + uspers = self.usperstep + ret = 0 + if style == self.INTERLEAVE: + uspers /= 2 + elif style == self.MICROSTEP: + uspers /= self.MICROSTEPS + steps *= self.MICROSTEPS + if self.debug: + print "steps = %d" % steps + + while steps > 0: + steps -= 1; + #if self.debug: + # print "step! %d uspers: %d" % (steps, uspers) + ret = self.onestep(dir, style); + time.sleep(uspers/1000000); # in seconde (uspers/1000 in ms) + self.steppingcounter += (uspers % 1000) + if self.steppingcounter >= 1000: + time.sleep(0.001) # 1 ms + self.steppingcounter -= 1000 + + + if style == self.MICROSTEP: + while ((ret != 0) and (ret != self.MICROSTEPS)): + ret = self.onestep(dir, style) + time.sleep(uspers/1000000); # in seconde (uspers/1000 in ms) + self.steppingcounter += (uspers % 1000) + if self.steppingcounter >= 1000: + time.sleep(0.001); # 1 ms + self.steppingcounter -= 1000 + + def onestep(self, dir, style): + ocra = 255 + ocrb = 255 + + # next determine what sort of stepping procedure we're up to + if style == self.SINGLE: + if ((self.currentstep/(self.MICROSTEPS/2)) % 2) > 0: # we're at an odd step, weird + if (dir == self.FORWARD): + self.currentstep += self.MICROSTEPS/2 + else: + self.currentstep -= self.MICROSTEPS/2 + else: # go to the next even step + if (dir == self.FORWARD): + self.currentstep += self.MICROSTEPS + else: + self.currentstep -= self.MICROSTEPS + + elif (style == self.DOUBLE): + if ((self.currentstep/(self.MICROSTEPS/2) % 2) == 0): # we're at an even step, weird + if (dir == self.FORWARD): + self.currentstep += self.MICROSTEPS/2 + else: + self.currentstep -= self.MICROSTEPS/2 + else: # go to the next odd step + if (dir == self.FORWARD): + self.currentstep += self.MICROSTEPS + else: + self.currentstep -= self.MICROSTEPS + + elif (style == self.INTERLEAVE): + if (dir == self.FORWARD): + self.currentstep += self.MICROSTEPS/2 + else: + self.currentstep -= self.MICROSTEPS/2 + + elif (style == self.MICROSTEP): + if (dir == self.FORWARD): + self.currentstep += 1 + else: + # BACKWARDS + self.currentstep -= 1 + + self.currentstep += self.MICROSTEPS*4; + self.currentstep %= self.MICROSTEPS*4; + + ocra = 0 + ocrb = 0 + + if ( (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS)): + ocra = self.__MICROSTEPCURVE[self.MICROSTEPS - self.currentstep] + ocrb = self.__MICROSTEPCURVE[self.currentstep] + elif ( (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2)): + ocra = self.__MICROSTEPCURVE[self.currentstep - self.MICROSTEPS] + ocrb = self.__MICROSTEPCURVE[self.MICROSTEPS*2 - self.currentstep] + elif ( (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3)): + ocra = self.__MICROSTEPCURVE[self.MICROSTEPS*3 - self.currentstep] + ocrb = self.__MICROSTEPCURVE[self.currentstep - self.MICROSTEPS*2] + elif ( (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4)): + ocra = self.__MICROSTEPCURVE[self.currentstep - self.MICROSTEPS*3] + ocrb = self.__MICROSTEPCURVE[self.MICROSTEPS*4 - self.currentstep] + + + self.currentstep += self.MICROSTEPS*4 + self.currentstep %= self.MICROSTEPS*4 + + if self.debug: + print "current step: %d" % self.currentstep + print " pwmA = %d pwmB = %d" % (ocra, ocrb) + + self.MC.setPWM(self.PWMApin, ocra*16) + self.MC.setPWM(self.PWMBpin, ocrb*16) + + # release all + latch_state = 0 # all motor pins to 0 + + #Serial.println(step, DEC); + if (style == self.MICROSTEP): + if ((self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS)): + latch_state |= 0x03 + if ((self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2)): + latch_state |= 0x06 + if ((self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3)): + latch_state |= 0x0C + if ((self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4)): + latch_state |= 0x09 + else: + switchValue = (self.currentstep/(self.MICROSTEPS/2)) + if switchValue == 0: + latch_state |= 0x1; # energize coil 1 only + elif switchValue == 1: + latch_state |= 0x3; # energize coil 1+2 + elif switchValue == 2: + latch_state |= 0x2; # energize coil 2 only + elif switchValue == 3: + latch_state |= 0x6; # energize coil 2+3 + elif switchValue == 4: + latch_state |= 0x4; # energize coil 3 only + elif switchValue == 5: + latch_state |= 0xC; # energize coil 3+4 + elif switchValue == 6: + latch_state |= 0x8; # energize coil 4 only + elif switchValue == 7: + latch_state |= 0x9; # energize coil 1+4 + + if self.debug: + print "Latch: 0x%x" % latch_state + + if (latch_state & 0x1): + # Serial.println(self.AIN2pin) + self.MC.setPin(self.AIN2pin, self.HIGH) + else: + self.MC.setPin(self.AIN2pin, self.LOW) + + if (latch_state & 0x2): + self.MC.setPin(self.BIN1pin, self.HIGH) + # Serial.println(self.BIN1pin) + else: + self.MC.setPin(self.BIN1pin, self.LOW) + + if (latch_state & 0x4): + self.MC.setPin(self.AIN1pin, self.HIGH) + # Serial.println(self.AIN1pin) + else: + self.MC.setPin(self.AIN1pin, self.LOW) + + if (latch_state & 0x8): + self.MC.setPin(self.BIN2pin, self.HIGH) + # Serial.println(self.BIN2pin) + else: + self.MC.setPin(self.BIN2pin, self.LOW) + + return self.currentstep diff --git a/Adafruit_MotorShield/DCMotorTest_Example.py b/Adafruit_MotorShield/DCMotorTest_Example.py new file mode 100644 index 00000000..7e489e99 --- /dev/null +++ b/Adafruit_MotorShield/DCMotorTest_Example.py @@ -0,0 +1,70 @@ +#!/usr/bin/python + +''' +For Motor Shield for Arduino V2 use with Raspberry Pi and level converter for i2c + +This is a test script for the Adafruit assembled Motor Shield for Arduino v2 +It won't work with v1.x motor shields! Only for the v2's with built in PWM +control + +For use with the Adafruit Motor Shield v2 +----> http://www.adafruit.com/products/1438 +''' + + +from Adafruit_MotorShield import Adafruit_MotorShield +from Adafruit_DCMotor import Adafruit_DCMotor +import time + +# Create the motor shield object with the default I2C address +AFMS = Adafruit_MotorShield() +# Or, create it with a different I2C address (say for stacking) +# AFMS = Adafruit_MotorShield(0x61); + +# Select which 'port' M1, M2, M3 or M4. In this case, M1 +myMotor = AFMS.getMotor(1) + +AFMS.begin() # create with the default frequency 1.6KHz +#AFMS.begin(1000); // OR with a different frequency, say 1KHz + +# Set the speed to start, from 0 (off) to 255 (max speed) +myMotor.setSpeed(150); # 10 rpm +myMotor.run(Adafruit_DCMotor.FORWARD) +# turn on motor +myMotor.run(Adafruit_DCMotor.RELEASE) + +try: + while (True): + + print "tick"; + + myMotor.run(Adafruit_DCMotor.FORWARD); + for i in range(0, 255): + myMotor.setSpeed(i) + time.sleep(0.01) + + for i in range(254, -1): + myMotor.setSpeed(i) + time.sleep(0.01) + + + print "tock"; + + myMotor.run(Adafruit_DCMotor.BACKWARD); + for i in range(0, 255): + myMotor.setSpeed(i) + time.sleep(0.01) + + for i in range(254, -1): + myMotor.setSpeed(i) + time.sleep(0.01) + + + print "tech"; + myMotor.run(Adafruit_DCMotor.RELEASE); + time.sleep(1) + +except KeyboardInterrupt: + myMotor.run(Adafruit_DCMotor.RELEASE) + print "Clean " + diff --git a/Adafruit_MotorShield/StepperTest_Example.py b/Adafruit_MotorShield/StepperTest_Example.py new file mode 100644 index 00000000..8cdc2694 --- /dev/null +++ b/Adafruit_MotorShield/StepperTest_Example.py @@ -0,0 +1,55 @@ +#!/usr/bin/python + +''' +For Motor Shield for Arduino V2 use with Raspberry Pi and level converter for i2c + +This is a test script for the Adafruit assembled Motor Shield for Arduino v2 +It won't work with v1.x motor shields! Only for the v2's with built in PWM +control + +For use with the Adafruit Motor Shield v2 +----> http://www.adafruit.com/products/1438 +''' + + +from Adafruit_MotorShield import Adafruit_MotorShield +from Adafruit_StepperMotor import Adafruit_StepperMotor +import time + +# Create the motor shield object with the default I2C address +AFMS = Adafruit_MotorShield() +# Or, create it with a different I2C address (say for stacking) +# AFMS = Adafruit_MotorShield(0x61); + +# Connect a stepper motor with 200 steps per revolution (1.8 degree) +# to motor port #2 (M3 and M4) +myMotor = AFMS.getStepper(200, 2) + +AFMS.begin() # create with the default frequency 1.6KHz +#AFMS.begin(1000); // OR with a different frequency, say 1KHz + +myMotor.setSpeed(10); # 10 rpm + +try: + while (True): + + print "Single coil steps" + myMotor.step(100, Adafruit_StepperMotor.FORWARD, Adafruit_StepperMotor.SINGLE); + myMotor.step(100, Adafruit_StepperMotor.BACKWARD, Adafruit_StepperMotor.SINGLE); + + print "Double coil steps" + myMotor.step(100, Adafruit_StepperMotor.FORWARD, Adafruit_StepperMotor.DOUBLE) + myMotor.step(100, Adafruit_StepperMotor.BACKWARD, Adafruit_StepperMotor.DOUBLE) + + print "Interleave coil steps" + myMotor.step(100, Adafruit_StepperMotor.FORWARD, Adafruit_StepperMotor.INTERLEAVE); + myMotor.step(100, Adafruit_StepperMotor.BACKWARD, Adafruit_StepperMotor.INTERLEAVE); + + print "Microstep steps" + myMotor.step(50, Adafruit_StepperMotor.FORWARD, Adafruit_StepperMotor.MICROSTEP); + myMotor.step(50, Adafruit_StepperMotor.BACKWARD, Adafruit_StepperMotor.MICROSTEP); + +except KeyboardInterrupt: + myMotor.release() + print "Clean " +