h3n3/darkwater_python_640
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #1 from darkwaterfoundation/dev

Browse Source 
Update from Dev
This commit is contained in:
shrkey
2016-09-17 21:36:56 +01:00
committed by GitHub
parent 1533b9cdf0 3cfca22056
commit e23067e7fc
2 changed files with 220 additions and 258 deletions
Download Patch File Download Diff File Expand all files Collapse all files

80
640steppertest.py
View File

@@ -2,83 +2,15 @@ import time
from darkwater_640.darkwater_640 import dw_Controller, dw_Servo, dw_Motor from darkwater_640.darkwater_640 import dw_Controller, dw_Servo, dw_Motor
dw = dw_Controller( addr=0x60 ) dw = dw_Controller( addr=0x60 )
m1 = dw.getMotor(1) s1 = dw.getStepper(1)
m2 = dw.getMotor(2)
m3 = dw.getMotor(3) m3 = dw.getMotor(3)
m4 = dw.getMotor(4)
m5 = dw.getMotor(5)
m6 = dw.getMotor(6)
m1.off() s1.off()
m2.off()
m3.off() m3.off()
m4.off()
m5.off()
m6.off()
time.sleep(1) 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) m3.setMotorSpeed(-255)
time.sleep(1) s1.oneStep( dw_Controller.FORWARD, dw_Controller.SINGLE);
print "Motor 4"
m4.setMotorSpeed(-255) s1.off()
time.sleep(1) m3.off()
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()

398
darkwater_640/darkwater_640.py
View File

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