diff --git a/darkwater_640/darkwater_640.py b/darkwater_640/darkwater_640.py index 4fae5d6..ffef84c 100644 --- a/darkwater_640/darkwater_640.py +++ b/darkwater_640/darkwater_640.py @@ -5,194 +5,200 @@ from PCA9685 import PCA9685 import time import math -# class dw_Stepper: -# MICROSTEPS = 8 -# MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255] +class dw_Stepper: + MICROSTEPS = 8 + MICROSTEP_CURVE = [0, 50, 98, 142, 180, 212, 236, 250, 255] -# #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] + #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] -# def __init__(self, controller, num, steps=200): -# self.speed = 0 -# self.MC = controller -# self.motornum = num -# modepin = in1 = in2 = 0 + def __init__(self, controller, num, steps=200): + self.speed = 0 + self.MC = controller + self.motornum = num + modepin = in1 = in2 = 0 -# self.revsteps = steps -# self.sec_per_step = 0.1 -# self.steppingcounter = 0 -# self.currentstep = 0 + self.revsteps = steps + self.sec_per_step = 0.1 + self.steppingcounter = 0 + self.currentstep = 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') + 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.PHpinA = ain2 -# self.ENpinA = ain1 -# self.PHpinB = bin2 -# self.ENpinB = bin1 -# # switch off both drivers -# self.run(dw_Controller.RELEASE, 0) + self.PHpinA = ain2 + self.ENpinA = ain1 + self.PHpinB = bin2 + self.ENpinB = bin1 -# 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) + # switch off both drivers + self.run(dw_Controller.RELEASE, 0) -# def off(self): -# self.run(dw_Controller.RELEASE, 0) + 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) -# def setSpeed(self, rpm): -# self.sec_per_step = 60.0 / (self.revsteps * rpm) -# self.steppingcounter = 0 + def off(self): + self.run(dw_Controller.RELEASE, 0) -# def oneStep(self, dir, style): -# pwm_a = pwm_b = 255 + def setSpeed(self, rpm): + self.sec_per_step = 60.0 / (self.revsteps * rpm) + self.steppingcounter = 0 -# # 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 oneStep(self, dir, style): + pwm_a = pwm_b = 255 -# if (style == dw_Controller.MICROSTEP): -# if (dir == dw_Controller.FORWARD): -# self.currentstep += 1 -# else: -# self.currentstep -= 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 -# # go to next 'step' and wrap around -# self.currentstep += self.MICROSTEPS * 4 -# self.currentstep %= self.MICROSTEPS * 4 + if (style == dw_Controller.MICROSTEP): + if (dir == dw_Controller.FORWARD): + self.currentstep += 1 + else: + self.currentstep -= 1 -# 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 + + 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) + # 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)] -# #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]) + #print "coils state = " + str(coils) + self.MC.setPin(self.PHpinA, coils[0]) #ain2 + self.MC.setPin(self.ENpinB, coils[1]) #bin1 + self.MC.setPin(self.ENpinA, coils[2]) #ain1 + self.MC.setPin(self.PHpinB, coils[3]) #bin2 -# return self.currentstep + #self.PHpinA = ain2 + # self.ENpinA = ain1 + # self.PHpinB = bin2 + # self.ENpinB = bin1 -# def step(self, steps, direction, stepstyle): -# s_per_s = self.sec_per_step -# lateststep = 0 + return self.currentstep -# 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 + def step(self, steps, direction, stepstyle): + s_per_s = self.sec_per_step + lateststep = 0 -# print s_per_s, " sec per step" + 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 -# for s in range(steps): -# lateststep = self.oneStep(direction, stepstyle) -# time.sleep(s_per_s) + print s_per_s, " sec per step" -# 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) + 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) class dw_Motor: def __init__(self, controller, num): @@ -333,12 +339,13 @@ 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 + #GPIO.output(27, GPIO.HIGH) # set for en/phase mode - low = in/in mode + GPIO.output(27, GPIO.LOW) # set 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 setPin(self, pin, value): if (pin < 0) or (pin > 15):