|
28 | 28 | * Author(s): Tony DiCola, Scott Shawcroft
|
29 | 29 | """
|
30 | 30 |
|
31 |
| -# imports |
| 31 | +import math |
| 32 | + |
| 33 | +from micropython import const |
32 | 34 |
|
33 | 35 | __version__ = "0.0.0-auto.0"
|
34 |
| -__repo__ = "https://github.com/adafruit/Adafruit_CircuitPython_motor.git" |
| 36 | +__repo__ = "https://github.com/adafruit/Adafruit_CircuitPython_Motor.git" |
35 | 37 |
|
36 | 38 | # Stepper Motor Shield/Wing Driver
|
37 | 39 | # Based on Adafruit Motorshield library:
|
38 | 40 | # https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library
|
39 | 41 |
|
40 |
| - |
41 | 42 | # Constants that specify the direction and style of steps.
|
42 | 43 | FORWARD = const(1)
|
| 44 | +"""Step forward""" |
43 | 45 | BACKWARD = const(2)
|
| 46 | +""""Step backward""" |
44 | 47 | SINGLE = const(1)
|
| 48 | +"""Step so that each step only activates a single coil""" |
45 | 49 | DOUBLE = const(2)
|
| 50 | +"""Step so that each step only activates two coils to produce more torque.""" |
46 | 51 | INTERLEAVE = const(3)
|
| 52 | +"""Step half a step to alternate between single coil and double coil steps.""" |
47 | 53 | MICROSTEP = const(4)
|
| 54 | +"""Step a fraction of a step by partially activating two neighboring coils. Step size is determined |
| 55 | + by ``microsteps`` constructor argument.""" |
48 | 56 |
|
49 |
| -# Not a const so users can change this global to 8 or 16 to change step size |
50 |
| -MICROSTEPS = 16 |
51 |
| - |
52 |
| -# Microstepping curves (these are constants but need to be tuples/indexable): |
53 |
| -_MICROSTEPCURVE8 = (0, 50, 98, 142, 180, 212, 236, 250, 255) |
54 |
| -_MICROSTEPCURVE16 = (0, 25, 50, 74, 98, 120, 141, 162, 180, 197, 212, 225, 236, 244, 250, 253, 255) |
55 |
| - |
56 |
| -# Define PWM outputs for each of two available steppers. |
57 |
| -# Each tuple defines for a stepper: pwma, ain2, ain1, pwmb, bin2, bin1 |
58 |
| -_STEPPERS = ((8, 9, 10, 13, 12, 11), (2, 3, 4, 7, 6, 5)) |
| 57 | +class StepperMotor: |
| 58 | + """A bipolar stepper motor or four coil unipolar motor. |
| 59 | +
|
| 60 | + :param ~pulseio.PWMOut ain1: `PWMOut`-compatible output connected to the driver for the first |
| 61 | + coil (unipolar) or first input to first coil (bipolar). |
| 62 | + :param ~pulseio.PWMOut ain2: `PWMOut`-compatible output connected to the driver for the third |
| 63 | + coil (unipolar) or second input to first coil (bipolar). |
| 64 | + :param ~pulseio.PWMOut bin1: `PWMOut`-compatible output connected to the driver for the second |
| 65 | + coil (unipolar) or second input to second coil (bipolar). |
| 66 | + :param ~pulseio.PWMOut bin2: `PWMOut`-compatible output connected to the driver for the fourth |
| 67 | + coil (unipolar) or second input to second coil (bipolar). |
| 68 | + :param int microsteps: Number of microsteps between full steps. Must be at least 2 and even. |
| 69 | + """ |
| 70 | + def __init__(self, ain1, ain2, bin1, bin2, *, microsteps=16): |
| 71 | + self._coil = (ain2, bin1, ain1, bin2) |
| 72 | + |
| 73 | + self._current_microstep = 0 |
| 74 | + if microsteps < 2: |
| 75 | + raise ValueError("Microsteps must be at least 2") |
| 76 | + if microsteps % 2 == 1: |
| 77 | + raise ValueError("Microsteps must be even") |
| 78 | + self._microsteps = microsteps |
| 79 | + self._curve = [int(round(0xffff * math.sin(math.pi / (2 * microsteps) * i))) |
| 80 | + for i in range(microsteps + 1)] |
| 81 | + self._update_coils() |
| 82 | + |
| 83 | + def _update_coils(self, *, microstepping=False): |
| 84 | + duty_cycles = [0, 0, 0, 0] |
| 85 | + trailing_coil = (self._current_microstep // self._microsteps) % 4 |
| 86 | + leading_coil = (trailing_coil + 1) % 4 |
| 87 | + microstep = self._current_microstep % self._microsteps |
| 88 | + duty_cycles[leading_coil] = self._curve[microstep] |
| 89 | + duty_cycles[trailing_coil] = self._curve[self._microsteps - microstep] |
| 90 | + |
| 91 | + # This ensures DOUBLE steps use full torque. Without it, we'd use partial torque from the |
| 92 | + # microstepping curve (0xb504). |
| 93 | + if not microstepping and (duty_cycles[leading_coil] == duty_cycles[trailing_coil] and |
| 94 | + duty_cycles[leading_coil] > 0): |
| 95 | + duty_cycles[leading_coil] = 0xffff |
| 96 | + duty_cycles[trailing_coil] = 0xffff |
59 | 97 |
|
| 98 | + # Energize coils as appropriate: |
| 99 | + for i in range(4): |
| 100 | + print(i, hex(duty_cycles[i])) |
| 101 | + self._coil[i].duty_cycle = duty_cycles[i] |
60 | 102 |
|
61 |
| -class StepperMotor: |
62 |
| - def __init__(self, pca, pwma, ain2, ain1, pwmb, bin2, bin1): |
63 |
| - self.pca9685 = pca |
64 |
| - self.pwma = pwma |
65 |
| - self.ain2 = ain2 |
66 |
| - self.ain1 = ain1 |
67 |
| - self.pwmb = pwmb |
68 |
| - self.bin2 = bin2 |
69 |
| - self.bin1 = bin1 |
70 |
| - self.currentstep = 0 |
71 |
| - |
72 |
| - def _pwm(self, pin, value): |
73 |
| - if value > 4095: |
74 |
| - self.pca9685.pwm(pin, 4096, 0) |
75 |
| - else: |
76 |
| - self.pca9685.pwm(pin, 0, value) |
| 103 | + def onestep(self, *, direction=FORWARD, style=SINGLE): |
| 104 | + """Performs one step of a particular style. The actual rotation amount will vary by style. |
| 105 | + `SINGLE` and `DOUBLE` will normal cause a full step rotation. `INTERLEAVE` will normally |
| 106 | + do a half step rotation. `MICROSTEP` will perform the smallest configured step. |
77 | 107 |
|
78 |
| - def _pin(self, pin, value): |
79 |
| - if value: |
80 |
| - self.pca9685.pwm(pin, 4096, 0) |
81 |
| - else: |
82 |
| - self.pca9685.pwm(pin, 0, 0) |
| 108 | + When step styles are mixed, subsequent `SINGLE`, `DOUBLE` or `INTERLEAVE` steps may be |
| 109 | + less than normal in order to align to the desired style's pattern. |
83 | 110 |
|
84 |
| - def onestep(self, direction, style): |
85 |
| - ocra = 255 |
86 |
| - ocrb = 255 |
| 111 | + :param int direction: Either `FORWARD` or `BACKWARD` |
| 112 | + :param int style: `SINGLE`, `DOUBLE`, `INTERLEAVE`""" |
87 | 113 | # Adjust current steps based on the direction and type of step.
|
88 |
| - if style == SINGLE: |
89 |
| - if (self.currentstep//(MICROSTEPS//2)) % 2: |
90 |
| - if direction == FORWARD: |
91 |
| - self.currentstep += MICROSTEPS//2 |
92 |
| - else: |
93 |
| - self.currentstep -= MICROSTEPS//2 |
94 |
| - else: |
95 |
| - if direction == FORWARD: |
96 |
| - self.currentstep += MICROSTEPS |
97 |
| - else: |
98 |
| - self.currentstep -= MICROSTEPS |
99 |
| - elif style == DOUBLE: |
100 |
| - if not (self.currentstep//(MICROSTEPS//2)) % 2: |
101 |
| - if direction == FORWARD: |
102 |
| - self.currentstep += MICROSTEPS//2 |
103 |
| - else: |
104 |
| - self.currentstep -= MICROSTEPS//2 |
105 |
| - else: |
106 |
| - if direction == FORWARD: |
107 |
| - self.currentstep += MICROSTEPS |
108 |
| - else: |
109 |
| - self.currentstep -= MICROSTEPS |
110 |
| - elif style == INTERLEAVE: |
111 |
| - if direction == FORWARD: |
112 |
| - self.currentstep += MICROSTEPS//2 |
113 |
| - else: |
114 |
| - self.currentstep -= MICROSTEPS//2 |
115 |
| - elif style == MICROSTEP: |
116 |
| - if direction == FORWARD: |
117 |
| - self.currentstep += 1 |
118 |
| - else: |
119 |
| - self.currentstep -= 1 |
120 |
| - self.currentstep += MICROSTEPS*4 |
121 |
| - self.currentstep %= MICROSTEPS*4 |
122 |
| - ocra = 0 |
123 |
| - ocrb = 0 |
124 |
| - if MICROSTEPS == 8: |
125 |
| - curve = _MICROSTEPCURVE8 |
126 |
| - elif MICROSTEPS == 16: |
127 |
| - curve = _MICROSTEPCURVE16 |
128 |
| - else: |
129 |
| - raise RuntimeError('MICROSTEPS must be 8 or 16!') |
130 |
| - if 0 <= self.currentstep < MICROSTEPS: |
131 |
| - ocra = curve[MICROSTEPS - self.currentstep] |
132 |
| - ocrb = curve[self.currentstep] |
133 |
| - elif MICROSTEPS <= self.currentstep < MICROSTEPS*2: |
134 |
| - ocra = curve[self.currentstep - MICROSTEPS] |
135 |
| - ocrb = curve[MICROSTEPS*2 - self.currentstep] |
136 |
| - elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3: |
137 |
| - ocra = curve[MICROSTEPS*3 - self.currentstep] |
138 |
| - ocrb = curve[self.currentstep - MICROSTEPS*2] |
139 |
| - elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4: |
140 |
| - ocra = curve[self.currentstep - MICROSTEPS*3] |
141 |
| - ocrb = curve[MICROSTEPS*4 - self.currentstep] |
142 |
| - self.currentstep += MICROSTEPS*4 |
143 |
| - self.currentstep %= MICROSTEPS*4 |
144 |
| - # Set PWM outputs. |
145 |
| - self._pwm(self.pwma, ocra*16) |
146 |
| - self._pwm(self.pwmb, ocrb*16) |
147 |
| - latch_state = 0 |
148 |
| - # Determine which coils to energize: |
| 114 | + step_size = 0 |
149 | 115 | if style == MICROSTEP:
|
150 |
| - if 0 <= self.currentstep < MICROSTEPS: |
151 |
| - latch_state |= 0x3 |
152 |
| - elif MICROSTEPS <= self.currentstep < MICROSTEPS*2: |
153 |
| - latch_state |= 0x6 |
154 |
| - elif MICROSTEPS*2 <= self.currentstep < MICROSTEPS*3: |
155 |
| - latch_state |= 0xC |
156 |
| - elif MICROSTEPS*3 <= self.currentstep < MICROSTEPS*4: |
157 |
| - latch_state |= 0x9 |
| 116 | + step_size = 1 |
158 | 117 | else:
|
159 |
| - latch_step = self.currentstep//(MICROSTEPS//2) |
160 |
| - if latch_step == 0: |
161 |
| - latch_state |= 0x1 # energize coil 1 only |
162 |
| - elif latch_step == 1: |
163 |
| - latch_state |= 0x3 # energize coil 1+2 |
164 |
| - elif latch_step == 2: |
165 |
| - latch_state |= 0x2 # energize coil 2 only |
166 |
| - elif latch_step == 3: |
167 |
| - latch_state |= 0x6 # energize coil 2+3 |
168 |
| - elif latch_step == 4: |
169 |
| - latch_state |= 0x4 # energize coil 3 only |
170 |
| - elif latch_step == 5: |
171 |
| - latch_state |= 0xC # energize coil 3+4 |
172 |
| - elif latch_step == 6: |
173 |
| - latch_state |= 0x8 # energize coil 4 only |
174 |
| - elif latch_step == 7: |
175 |
| - latch_state |= 0x9 # energize coil 1+4 |
176 |
| - # Energize coils as appropriate: |
177 |
| - if latch_state & 0x1: |
178 |
| - self._pin(self.ain2, True) |
179 |
| - else: |
180 |
| - self._pin(self.ain2, False) |
181 |
| - if latch_state & 0x2: |
182 |
| - self._pin(self.bin1, True) |
183 |
| - else: |
184 |
| - self._pin(self.bin1, False) |
185 |
| - if latch_state & 0x4: |
186 |
| - self._pin(self.ain1, True) |
187 |
| - else: |
188 |
| - self._pin(self.ain1, False) |
189 |
| - if latch_state & 0x8: |
190 |
| - self._pin(self.bin2, True) |
| 118 | + half_step = self._microsteps // 2 |
| 119 | + full_step = self._microsteps |
| 120 | + # Its possible the previous steps were MICROSTEPS so first align with the interleave |
| 121 | + # pattern. |
| 122 | + additional_microsteps = self._current_microstep % half_step |
| 123 | + if additional_microsteps != 0: |
| 124 | + # We set _current_microstep directly because our step size varies depending on the |
| 125 | + # direction. |
| 126 | + if direction == FORWARD: |
| 127 | + self._current_microstep += half_step - additional_microsteps |
| 128 | + else: |
| 129 | + self._current_microstep -= additional_microsteps |
| 130 | + step_size = 0 |
| 131 | + elif style == INTERLEAVE: |
| 132 | + step_size = half_step |
| 133 | + |
| 134 | + current_interleave = self._current_microstep // half_step |
| 135 | + if ((style == SINGLE and current_interleave % 2 == 1) or |
| 136 | + (style == DOUBLE and current_interleave % 2 == 0)): |
| 137 | + step_size = half_step |
| 138 | + elif style == SINGLE or style == DOUBLE: |
| 139 | + step_size = full_step |
| 140 | + |
| 141 | + print(step_size, MICROSTEP) |
| 142 | + |
| 143 | + if direction == FORWARD: |
| 144 | + self._current_microstep += step_size |
191 | 145 | else:
|
192 |
| - self._pin(self.bin2, False) |
193 |
| - return self.currentstep |
| 146 | + self._current_microstep -= step_size |
194 | 147 |
|
| 148 | + print(self._current_microstep) |
195 | 149 |
|
196 |
| -class Steppers: |
197 |
| - def __init__(self, i2c, address=0x60, freq=1600): |
198 |
| - self.pca9685 = pca9685.PCA9685(i2c, address) |
199 |
| - self.pca9685.freq(freq) |
| 150 | + # Now that we know our target microstep we can determine how to energize the four coils. |
| 151 | + self._update_coils(microstepping=style == MICROSTEP) |
200 | 152 |
|
201 |
| - def get_stepper(self, num): |
202 |
| - pwma, ain2, ain1, pwmb, bin2, bin1 = _STEPPERS[num] |
203 |
| - return StepperMotor(self.pca9685, pwma, ain2, ain1, pwmb, bin2, bin1) |
| 153 | + return self._current_microstep |
0 commit comments