diff --git a/640steppertest.py b/640steppertest.py index 568d40f..27c5c17 100644 --- a/640steppertest.py +++ b/640steppertest.py @@ -2,83 +2,15 @@ import time from darkwater_640.darkwater_640 import dw_Controller, dw_Servo, dw_Motor dw = dw_Controller( addr=0x60 ) -m1 = dw.getMotor(1) -m2 = dw.getMotor(2) +s1 = dw.getStepper(1) m3 = dw.getMotor(3) -m4 = dw.getMotor(4) -m5 = dw.getMotor(5) -m6 = dw.getMotor(6) -m1.off() -m2.off() +s1.off() m3.off() -m4.off() -m5.off() -m6.off() time.sleep(1) -##time.sleep(10) -print "Set forward - " -print "Motor 1" -m1.setMotorSpeed(255) -time.sleep(1) -print "Motor 2" -m2.setMotorSpeed(255) -time.sleep(1) -print "Motor 3" -m3.setMotorSpeed(255) -time.sleep(1) -print "Motor 4" -m4.setMotorSpeed(255) -time.sleep(1) -print "Motor 5" -m5.setMotorSpeed(255) -time.sleep(1) -print "Motor 6" -m6.setMotorSpeed(255) -time.sleep(1) -print "Stopping - " -print "Motor 1" -m1.setMotorSpeed(0) -time.sleep(1) -print "Motor 2" -m2.setMotorSpeed(0) -time.sleep(1) -print "Motor 3" -m3.setMotorSpeed(0) -time.sleep(1) -print "Motor 4" -m4.setMotorSpeed(0) -time.sleep(1) -print "Motor 5" -m5.setMotorSpeed(0) -time.sleep(1) -print "Motor 6" -m6.setMotorSpeed(0) -time.sleep(1) -print "Set reverse - " -print "Motor 1" -m1.setMotorSpeed(-255) -time.sleep(1) -print "Motor 2" -m2.setMotorSpeed(-255) -time.sleep(1) -print "Motor 3" m3.setMotorSpeed(-255) -time.sleep(1) -print "Motor 4" -m4.setMotorSpeed(-255) -time.sleep(1) -print "Motor 5" -m5.setMotorSpeed(-255) -time.sleep(1) -print "Motor 6" -m6.setMotorSpeed(-255) -time.sleep(1) -print "All off" -m1.off() -m2.off() -m3.off() -m4.off() -m5.off() -m6.off() +s1.oneStep( dw_Controller.FORWARD, dw_Controller.SINGLE); + +s1.off() +m3.off() \ No newline at end of file diff --git a/darkwater_640/darkwater_640.py b/darkwater_640/darkwater_640.py index 4fae5d6..f63892e 100644 --- a/darkwater_640/darkwater_640.py +++ b/darkwater_640/darkwater_640.py @@ -5,194 +5,186 @@ from PCA9685 import PCA9685 import time import math -# class dw_Stepper: -# MICROSTEPS = 8 -# MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255] +# Stepper motor code based on Adafruit Python Library for DC + Stepper Motor HAT +# Written by Limor Fried for Adafruit Industries. MIT license. -# #MICROSTEPS = 16 -# # a sinusoidal curve NOT LINEAR! -# #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255] +class dw_Stepper: + MICROSTEPS = 8 + MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255] -# def __init__(self, controller, num, steps=200): -# self.speed = 0 -# self.MC = controller -# self.motornum = num -# modepin = in1 = in2 = 0 + #MICROSTEPS = 16 + # a sinusoidal curve NOT LINEAR! + #MICROSTEP_CURVE = [0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255] -# self.revsteps = steps -# self.sec_per_step = 0.1 -# self.steppingcounter = 0 -# self.currentstep = 0 + def __init__(self, controller, num, steps=200, forcemode=False): + self.speed = 0 + self.MC = controller + self.motornum = num + modepin = in1 = in2 = 0 -# if (num == 0): -# ain2 = 2 #phase -# ain1 = 3 #enable -# bin2 = 4 #phase -# bin1 = 5 #enable -# elif (num == 1): -# ain2 = 6 #phase -# ain1 = 7 #enable -# bin2 = 8 #phase -# bin1 = 9 #enable -# elif (num == 2): -# ain2 = 10 #phase -# ain1 = 11 #enable -# bin2 = 12 #phase -# bin1 = 13 #enable -# else: -# raise NameError('MotorHAT Stepper must be between 1 and 3 inclusive') + self.revsteps = steps + self.sec_per_step = 0.1 + self.steppingcounter = 0 + self.currentstep = 0 -# self.PHpinA = ain2 -# self.ENpinA = ain1 -# self.PHpinB = bin2 -# self.ENpinB = bin1 -# # switch off both drivers -# self.run(dw_Controller.RELEASE, 0) + if(self.MC.getMode() != dw_Controller.ININ and forcemode != True ): + raise NameError('Mode needs to be set to ININ for stepper mode operation') + else: + self.MC.setMode(dw_Controller.ININ) -# def run(self, command, speed = 0): -# if not self.MC: -# return -# if (command == dw_Controller.FORWARD): -# self.MC.setPin(self.PHpin, 0) -# self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) -# if (command == dw_Controller.BACKWARD): -# self.MC.setPin(self.PHpin, 1) -# self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) -# if (command == dw_Controller.RELEASE): -# self.MC.setPin(self.PHpinA, 0) -# self.MC.setPin(self.ENpinA, 0) -# self.MC.setPin(self.PHpinB, 0) -# self.MC.setPin(self.ENpinB, 0) + if (num == 0): + ain1 = 2 + ain2 = 3 + bin1 = 4 + bin2 = 5 + elif (num == 1): + ain1 = 6 + ain2 = 7 + bin1 = 8 + bin2 = 9 + elif (num == 2): + ain1 = 10 + ain2 = 11 + bin1 = 12 + bin2 = 13 + else: + raise NameError('Stepper must be between 1 and 3 inclusive') -# def off(self): -# self.run(dw_Controller.RELEASE, 0) + self.ain1 = ain1 + self.ain2 = ain2 + self.bin1 = bin1 + self.bin2 = bin2 -# def setSpeed(self, rpm): -# self.sec_per_step = 60.0 / (self.revsteps * rpm) -# self.steppingcounter = 0 + # switch off both drivers + self.run(dw_Controller.STOP, 0) -# def oneStep(self, dir, style): -# pwm_a = pwm_b = 255 + def run(self, command, speed = 0): + if not self.MC: + return + + if (command == dw_Controller.STOP): + self.MC.setPin(self.ain1, 1) + self.MC.setPin(self.ain2, 1) + self.MC.setPin(self.bin1, 1) + self.MC.setPin(self.bin2, 1) -# # first determine what sort of stepping procedure we're up to -# if (style == dw_Controller.SINGLE): -# if ((self.currentstep/(self.MICROSTEPS/2)) % 2): -# # we're at an odd step, weird -# if (dir == dw_Controller.FORWARD): -# self.currentstep += self.MICROSTEPS/2 -# else: -# self.currentstep -= self.MICROSTEPS/2 -# else: -# # go to next even step -# if (dir == dw_Controller.FORWARD): -# self.currentstep += self.MICROSTEPS -# else: -# self.currentstep -= self.MICROSTEPS -# if (style == dw_Controller.DOUBLE): -# if not (self.currentstep/(self.MICROSTEPS/2) % 2): -# # we're at an even step, weird -# if (dir == dw_Controller.FORWARD): -# self.currentstep += self.MICROSTEPS/2 -# else: -# self.currentstep -= self.MICROSTEPS/2 -# else: -# # go to next odd step -# if (dir == dw_Controller.FORWARD): -# self.currentstep += self.MICROSTEPS -# else: -# self.currentstep -= self.MICROSTEPS -# if (style == dw_Controller.INTERLEAVE): -# if (dir == dw_Controller.FORWARD): -# self.currentstep += self.MICROSTEPS/2 -# else: -# self.currentstep -= self.MICROSTEPS/2 + def off(self): + self.run(dw_Controller.STOP, 0) -# if (style == dw_Controller.MICROSTEP): -# if (dir == dw_Controller.FORWARD): -# self.currentstep += 1 -# else: -# self.currentstep -= 1 + def setSpeed(self, rpm): + self.sec_per_step = 60.0 / (self.revsteps * rpm) + self.steppingcounter = 0 -# # go to next 'step' and wrap around -# self.currentstep += self.MICROSTEPS * 4 -# self.currentstep %= self.MICROSTEPS * 4 + def oneStep(self, dir, style): + pwm_a = pwm_b = 255 -# pwm_a = pwm_b = 0 -# if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS): -# pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep] -# pwm_b = self.MICROSTEP_CURVE[self.currentstep] -# elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2): -# pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS] -# pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep] -# elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3): -# pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep] -# pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2] -# elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4): -# pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3] -# pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep] + # first determine what sort of stepping procedure we're up to + if (style == dw_Controller.SINGLE): + if ((self.currentstep/(self.MICROSTEPS/2)) % 2): + # we're at an odd step, weird + if (dir == dw_Controller.FORWARD): + self.currentstep += self.MICROSTEPS/2 + else: + self.currentstep -= self.MICROSTEPS/2 + else: + # go to next even step + if (dir == dw_Controller.FORWARD): + self.currentstep += self.MICROSTEPS + else: + self.currentstep -= self.MICROSTEPS + if (style == dw_Controller.DOUBLE): + if not (self.currentstep/(self.MICROSTEPS/2) % 2): + # we're at an even step, weird + if (dir == dw_Controller.FORWARD): + self.currentstep += self.MICROSTEPS/2 + else: + self.currentstep -= self.MICROSTEPS/2 + else: + # go to next odd step + if (dir == dw_Controller.FORWARD): + self.currentstep += self.MICROSTEPS + else: + self.currentstep -= self.MICROSTEPS + + if (style == dw_Controller.MICROSTEP): + if (dir == dw_Controller.FORWARD): + self.currentstep += 1 + else: + self.currentstep -= 1 + + # go to next 'step' and wrap around + self.currentstep += self.MICROSTEPS * 4 + self.currentstep %= self.MICROSTEPS * 4 + + pwm_a = pwm_b = 0 + if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS): + pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS - self.currentstep] + pwm_b = self.MICROSTEP_CURVE[self.currentstep] + elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2): + pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS] + pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*2 - self.currentstep] + elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3): + pwm_a = self.MICROSTEP_CURVE[self.MICROSTEPS*3 - self.currentstep] + pwm_b = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*2] + elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4): + pwm_a = self.MICROSTEP_CURVE[self.currentstep - self.MICROSTEPS*3] + pwm_b = self.MICROSTEP_CURVE[self.MICROSTEPS*4 - self.currentstep] -# # go to next 'step' and wrap around -# self.currentstep += self.MICROSTEPS * 4 -# self.currentstep %= self.MICROSTEPS * 4 + # go to next 'step' and wrap around + self.currentstep += self.MICROSTEPS * 4 + self.currentstep %= self.MICROSTEPS * 4 -# # only really used for microstepping, otherwise always on! -# self.MC._pwm.setPWM(self.PWMA, 0, pwm_a*16) -# self.MC._pwm.setPWM(self.PWMB, 0, pwm_b*16) + # set up coil energizing! + coils = [0, 0, 0, 0] -# # set up coil energizing! -# coils = [0, 0, 0, 0] + if (style == dw_Controller.MICROSTEP): + if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS): + coils = [1, 1, 0, 0] + elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2): + coils = [0, 1, 1, 0] + elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3): + coils = [0, 0, 1, 1] + elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4): + coils = [1, 0, 0, 1] + else: + step2coils = [ [1, 0, 0, 0], + [1, 1, 0, 0], + [0, 1, 0, 0], + [0, 1, 1, 0], + [0, 0, 1, 0], + [0, 0, 1, 1], + [0, 0, 0, 1], + [1, 0, 0, 1] ] + coils = step2coils[self.currentstep/(self.MICROSTEPS/2)] -# if (style == dw_Controller.MICROSTEP): -# if (self.currentstep >= 0) and (self.currentstep < self.MICROSTEPS): -# coils = [1, 1, 0, 0] -# elif (self.currentstep >= self.MICROSTEPS) and (self.currentstep < self.MICROSTEPS*2): -# coils = [0, 1, 1, 0] -# elif (self.currentstep >= self.MICROSTEPS*2) and (self.currentstep < self.MICROSTEPS*3): -# coils = [0, 0, 1, 1] -# elif (self.currentstep >= self.MICROSTEPS*3) and (self.currentstep < self.MICROSTEPS*4): -# coils = [1, 0, 0, 1] -# else: -# step2coils = [ [1, 0, 0, 0], -# [1, 1, 0, 0], -# [0, 1, 0, 0], -# [0, 1, 1, 0], -# [0, 0, 1, 0], -# [0, 0, 1, 1], -# [0, 0, 0, 1], -# [1, 0, 0, 1] ] -# coils = step2coils[self.currentstep/(self.MICROSTEPS/2)] + self.MC.setPin(self.ain1, coils[0]) #ain2 + self.MC.setPin(self.bin1, coils[1]) #bin1 + self.MC.setPin(self.ain2, coils[2]) #ain1 + self.MC.setPin(self.bin2, coils[3]) #bin2 -# #print "coils state = " + str(coils) -# self.MC.setPin(self.AIN2, coils[0]) -# self.MC.setPin(self.BIN1, coils[1]) -# self.MC.setPin(self.AIN1, coils[2]) -# self.MC.setPin(self.BIN2, coils[3]) + return self.currentstep -# return self.currentstep + def step(self, steps, direction, stepstyle): + s_per_s = self.sec_per_step + lateststep = 0 -# def step(self, steps, direction, stepstyle): -# s_per_s = self.sec_per_step -# lateststep = 0 + if (stepstyle == dw_Controller.MICROSTEP): + s_per_s /= self.MICROSTEPS + steps *= self.MICROSTEPS -# if (stepstyle == dw_Controller.INTERLEAVE): -# s_per_s = s_per_s / 2.0 -# if (stepstyle == dw_Controller.MICROSTEP): -# s_per_s /= self.MICROSTEPS -# steps *= self.MICROSTEPS + print s_per_s, " sec per step" -# print s_per_s, " sec per step" + for s in range(steps): + lateststep = self.oneStep(direction, stepstyle) + time.sleep(s_per_s) -# for s in range(steps): -# lateststep = self.oneStep(direction, stepstyle) -# time.sleep(s_per_s) - -# if (stepstyle == dw_Controller.MICROSTEP): -# # this is an edge case, if we are in between full steps, lets just keep going -# # so we end on a full step -# while (lateststep != 0) and (lateststep != self.MICROSTEPS): -# lateststep = self.oneStep(dir, stepstyle) -# time.sleep(s_per_s) + if (stepstyle == dw_Controller.MICROSTEP): + # this is an edge case, if we are in between full steps, lets just keep going + # so we end on a full step + while (lateststep != 0) and (lateststep != self.MICROSTEPS): + lateststep = self.oneStep(dir, stepstyle) + time.sleep(s_per_s) class dw_Motor: def __init__(self, controller, num): @@ -222,40 +214,59 @@ class dw_Motor: else: raise NameError('Motors must be between 1 and 6 inclusive') + # for phase enable self.PHpin = in2 self.ENpin = in1 + # for in/in + self.IN2pin = in2 + self.IN1pin = in1 # switch off - self.run(dw_Controller.RELEASE, 0) + self.run(dw_Controller.STOP, 0) def setMotorSpeed(self, value): # Check for PWM values if(value > 1000) and (value < 1500): - self.run(dw_Controller.BACKWARD, round(translate(value,1500,1000, 0, 255))) + self.run(dw_Controller.REVERSE, round(translate(value,1500,1000, 0, 255))) if(value > 1500) and (value <= 2000): self.run(dw_Controller.FORWARD, round(translate(value, 1500, 2000, 0, 255))) if(value == 1500): - self.run(dw_Controller.RELEASE, 0) + self.run(dw_Controller.STOP, 0) if(value > 0) and (value <= 255): self.run(dw_Controller.FORWARD, value) if(value == 0): - self.run(dw_Controller.RELEASE, value) + self.run(dw_Controller.STOP, value) if(value < 0) and (value >= -255): - self.run(dw_Controller.BACKWARD, abs(value)) + self.run(dw_Controller.REVERSE, abs(value)) def run(self, command, speed = 0): if not self.MC: return - if (command == dw_Controller.FORWARD): - self.MC.setPin(self.PHpin, 0) - self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) - if (command == dw_Controller.BACKWARD): - self.MC.setPin(self.PHpin, 1) - self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) - if (command == dw_Controller.RELEASE): - self.MC.setPin(self.PHpin, 0) - self.MC.setPin(self.ENpin, 0) + if (self.MC.getMode() == dw_Controller.PHASE): + if (command == dw_Controller.FORWARD): + self.MC.setPin(self.PHpin, 0) + self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) + if (command == dw_Controller.REVERSE): + self.MC.setPin(self.PHpin, 1) + self.MC._pwm.set_pwm(self.ENpin, 0, speed*16) + if (command == dw_Controller.STOP): + self.MC.setPin(self.PHpin, 0) + self.MC.setPin(self.ENpin, 0) + else: ## IN/IN mode + if (command == dw_Controller.FORWARD): + self.MC.setPin(self.IN2pin, 0) + self.MC._pwm.set_pwm(self.IN1pin, 0, speed*16) + if (command == dw_Controller.REVERSE): + self.MC.setPin(self.IN1pin, 0) + self.MC._pwm.set_pwm(self.IN2pin, 0, speed*16) + if (command == dw_Controller.STOP): + self.MC.setPin(self.IN1pin, 1) + self.MC.setPin(self.IN2pin, 1) + if (command == dw_Controller.COAST): + self.MC.setPin(self.IN1pin, 0) + self.MC.setPin(self.IN2pin, 0) + def off(self): - self.run(dw_Controller.RELEASE, 0) + self.run(dw_Controller.STOP, 0) class dw_Servo: @@ -311,9 +322,11 @@ class dw_Servo: class dw_Controller: FORWARD = 1 - BACKWARD = 2 - BRAKE = 3 - RELEASE = 4 + REVERSE = 2 + BRAKEFORWARD = 3 + BRAKEREVERSE = 4 + STOP = 5 #brake + COAST = 6 # in/in only SINGLE = 1 DOUBLE = 2 @@ -333,12 +346,24 @@ class dw_Controller: GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup(27, GPIO.OUT) - GPIO.output(27, GPIO.HIGH) # set for en/phase mode - low = in/in mode + + self.setMode(self.ININ) # set high for en/phase mode - low = in/in mode self.motors = [ dw_Motor(self, m) for m in range(6) ] self.servos = [ dw_Servo(self, m, freq) for m in range(2) ] - #self.steppers = [ dw_Stepper(self, m) for m in range(3) ] + self.steppers = [ dw_Stepper(self, m) for m in range(3) ] + + def setMode(self, mode = 1): + if (mode == self.ININ): + self._mode = self.ININ + GPIO.output(27, GPIO.LOW) + else: + self._mode = self.PHASE + GPIO.output(27, GPIO.HIGH) + + def getMode(self): + return self._mode def setPin(self, pin, value): if (pin < 0) or (pin > 15): @@ -370,6 +395,11 @@ class dw_Controller: raise NameError('Servos must be between 1 and 2 inclusive') return self.servos[num-1] + def getStepper(self, num): + if (num < 1) or (num > 3): + raise NameError('Stepper motors must be between 1 and 3 inclusive') + return self.steppers[num-1] + def setAllPWM(self, value): if(value > 0): self._pwm.set_all_pwm(0, value)