This repository has been archived by the owner on Dec 16, 2018. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 38
/
stepper.py
170 lines (157 loc) · 6.17 KB
/
stepper.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
# Stepper Motor Shield/Wing Driver
# Based on Adafruit Motorshield library:
# https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
# Author: Tony DiCola
import pca9685
# Constants that specify the direction and style of steps.
FORWARD = const(1)
BACKWARD = const(2)
SINGLE = const(1)
DOUBLE = const(2)
INTERLEAVE = const(3)
MICROSTEP = const(4)
# Not a const so users can change this global to 8 or 16 to change step size
MICROSTEPS = 16
# Microstepping curves (these are constants but need to be tuples/indexable):
_MICROSTEPCURVE8 = (0, 50, 98, 142, 180, 212, 236, 250, 255)
_MICROSTEPCURVE16 = (0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255)
# Define PWM outputs for each of two available steppers.
# Each tuple defines for a stepper: pwma, ain2, ain1, pwmb, bin2, bin1
_STEPPERS = ((8, 9, 10, 13, 12, 11), (2, 3, 4, 7, 6, 5))
class StepperMotor:
def __init__(self, pca, pwma, ain2, ain1, pwmb, bin2, bin1):
self.pca9685 = pca
self.pwma = pwma
self.ain2 = ain2
self.ain1 = ain1
self.pwmb = pwmb
self.bin2 = bin2
self.bin1 = bin1
self.currentstep = 0
def _pwm(self, pin, value):
if value > 4095:
self.pca9685.pwm(pin, 4096, 0)
else:
self.pca9685.pwm(pin, 0, value)
def _pin(self, pin, value):
if value:
self.pca9685.pwm(pin, 4096, 0)
else:
self.pca9685.pwm(pin, 0, 0)
def onestep(self, direction, style):
ocra = 255
ocrb = 255
# Adjust current steps based on the direction and type of step.
if style == SINGLE:
if (self.currentstep//(MICROSTEPS//2)) % 2:
if direction == FORWARD:
self.currentstep += MICROSTEPS//2
else:
self.currentstep -= MICROSTEPS//2
else:
if direction == FORWARD:
self.currentstep += MICROSTEPS
else:
self.currentstep -= MICROSTEPS
elif style == DOUBLE:
if not (self.currentstep//(MICROSTEPS//2)) % 2:
if direction == FORWARD:
self.currentstep += MICROSTEPS//2
else:
self.currentstep -= MICROSTEPS//2
else:
if direction == FORWARD:
self.currentstep += MICROSTEPS
else:
self.currentstep -= MICROSTEPS
elif style == INTERLEAVE:
if direction == FORWARD:
self.currentstep += MICROSTEPS//2
else:
self.currentstep -= MICROSTEPS//2
elif style == MICROSTEP:
if direction == FORWARD:
self.currentstep += 1
else:
self.currentstep -= 1
self.currentstep += MICROSTEPS*4
self.currentstep %= MICROSTEPS*4
ocra = 0
ocrb = 0
if MICROSTEPS == 8:
curve = _MICROSTEPCURVE8
elif MICROSTEPS == 16:
curve = _MICROSTEPCURVE16
else:
raise RuntimeError('MICROSTEPS must be 8 or 16!')
if 0 <= self.currentstep < MICROSTEPS:
ocra = curve[MICROSTEPS - self.currentstep]
ocrb = curve[self.currentstep]
elif MICROSTEPS <= self.currentstep < MICROSTEPS*2:
ocra = curve[self.currentstep - MICROSTEPS]
ocrb = curve[MICROSTEPS*2 - self.currentstep]
elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3:
ocra = curve[MICROSTEPS*3 - self.currentstep]
ocrb = curve[self.currentstep - MICROSTEPS*2]
elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4:
ocra = curve[self.currentstep - MICROSTEPS*3]
ocrb = curve[MICROSTEPS*4 - self.currentstep]
self.currentstep += MICROSTEPS*4
self.currentstep %= MICROSTEPS*4
# Set PWM outputs.
self._pwm(self.pwma, ocra*16)
self._pwm(self.pwmb, ocrb*16)
latch_state = 0
# Determine which coils to energize:
if style == MICROSTEP:
if 0 <= self.currentstep < MICROSTEPS:
latch_state |= 0x3
elif MICROSTEPS <= self.currentstep < MICROSTEPS*2:
latch_state |= 0x6
elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3:
latch_state |= 0xC
elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4:
latch_state |= 0x9
else:
latch_step = self.currentstep//(MICROSTEPS//2)
if latch_step == 0:
latch_state |= 0x1 # energize coil 1 only
elif latch_step == 1:
latch_state |= 0x3 # energize coil 1+2
elif latch_step == 2:
latch_state |= 0x2 # energize coil 2 only
elif latch_step == 3:
latch_state |= 0x6 # energize coil 2+3
elif latch_step == 4:
latch_state |= 0x4 # energize coil 3 only
elif latch_step == 5:
latch_state |= 0xC # energize coil 3+4
elif latch_step == 6:
latch_state |= 0x8 # energize coil 4 only
elif latch_step == 7:
latch_state |= 0x9 # energize coil 1+4
# Energize coils as appropriate:
if latch_state & 0x1:
self._pin(self.ain2, True)
else:
self._pin(self.ain2, False)
if latch_state & 0x2:
self._pin(self.bin1, True)
else:
self._pin(self.bin1, False)
if latch_state & 0x4:
self._pin(self.ain1, True)
else:
self._pin(self.ain1, False)
if latch_state & 0x8:
self._pin(self.bin2, True)
else:
self._pin(self.bin2, False)
return self.currentstep
class Steppers:
def __init__(self, i2c, address=0x60, freq=1600):
self.pca9685 = pca9685.PCA9685(i2c, address)
self.pca9685.freq(freq)
def get_stepper(self, num):
pwma, ain2, ain1, pwmb, bin2, bin1 = _STEPPERS[num]
return StepperMotor(self.pca9685, pwma, ain2, ain1, pwmb, bin2, bin1)