PyTmcStepper 0.15.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- pytmcstepper-0.15.0.dist-info/METADATA +338 -0
- pytmcstepper-0.15.0.dist-info/RECORD +56 -0
- pytmcstepper-0.15.0.dist-info/WHEEL +5 -0
- pytmcstepper-0.15.0.dist-info/licenses/LICENSE +683 -0
- pytmcstepper-0.15.0.dist-info/top_level.txt +1 -0
- tmc_driver/__init__.py +96 -0
- tmc_driver/_tmc_exceptions.py +21 -0
- tmc_driver/_tmc_math.py +122 -0
- tmc_driver/_tmc_stallguard.py +340 -0
- tmc_driver/_tmc_stepperdriver.py +210 -0
- tmc_driver/_tmc_validation.py +29 -0
- tmc_driver/_tmc_xxxx.py +489 -0
- tmc_driver/com/__init__.py +64 -0
- tmc_driver/com/_tmc_com.py +148 -0
- tmc_driver/com/_tmc_com_spi.py +81 -0
- tmc_driver/com/_tmc_com_spi_base.py +159 -0
- tmc_driver/com/_tmc_com_spi_ftdi.py +48 -0
- tmc_driver/com/_tmc_com_spi_micropython.py +99 -0
- tmc_driver/com/_tmc_com_uart.py +113 -0
- tmc_driver/com/_tmc_com_uart_base.py +386 -0
- tmc_driver/com/_tmc_com_uart_micropython.py +115 -0
- tmc_driver/enable_control/__init__.py +55 -0
- tmc_driver/enable_control/_tmc_ec.py +59 -0
- tmc_driver/enable_control/_tmc_ec_pin.py +48 -0
- tmc_driver/enable_control/_tmc_ec_toff.py +30 -0
- tmc_driver/motion_control/__init__.py +111 -0
- tmc_driver/motion_control/_tmc_mc.py +280 -0
- tmc_driver/motion_control/_tmc_mc_int_ramp_generator.py +218 -0
- tmc_driver/motion_control/_tmc_mc_step_dir.py +415 -0
- tmc_driver/motion_control/_tmc_mc_step_pwm_dir.py +78 -0
- tmc_driver/motion_control/_tmc_mc_step_reg.py +36 -0
- tmc_driver/motion_control/_tmc_mc_vactual.py +221 -0
- tmc_driver/platform_utils.py +13 -0
- tmc_driver/reg/__init__.py +0 -0
- tmc_driver/reg/_tmc2209_reg.py +55 -0
- tmc_driver/reg/_tmc220x_reg.py +261 -0
- tmc_driver/reg/_tmc224x_reg.py +414 -0
- tmc_driver/reg/_tmc5160_reg.py +526 -0
- tmc_driver/reg/_tmc_reg.py +202 -0
- tmc_driver/reg/_tmc_shared_regs.py +314 -0
- tmc_driver/tmc_2208.py +17 -0
- tmc_driver/tmc_2209.py +75 -0
- tmc_driver/tmc_220x.py +383 -0
- tmc_driver/tmc_2240.py +340 -0
- tmc_driver/tmc_5160.py +344 -0
- tmc_driver/tmc_gpio/__init__.py +101 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_base.py +106 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_ftdi.py +162 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_gpiozero.py +135 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_micropython.py +108 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_periphery.py +87 -0
- tmc_driver/tmc_gpio/_tmc_gpio_board_rpi_gpio.py +182 -0
- tmc_driver/tmc_logger/__init__.py +15 -0
- tmc_driver/tmc_logger/_tmc_logger_base.py +111 -0
- tmc_driver/tmc_logger/_tmc_logger_cpython.py +155 -0
- tmc_driver/tmc_logger/_tmc_logger_micropython.py +52 -0
|
@@ -0,0 +1,415 @@
|
|
|
1
|
+
# pylint: disable=too-many-instance-attributes
|
|
2
|
+
# pylint: disable=too-many-arguments
|
|
3
|
+
# pylint: disable=too-many-branches
|
|
4
|
+
# pylint: disable=too-many-positional-arguments
|
|
5
|
+
"""
|
|
6
|
+
STEP/DIR Motion Control module
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
import time
|
|
10
|
+
import math
|
|
11
|
+
import threading
|
|
12
|
+
from ._tmc_mc import (
|
|
13
|
+
TmcMotionControl,
|
|
14
|
+
MovementAbsRel,
|
|
15
|
+
MovementPhase,
|
|
16
|
+
Direction,
|
|
17
|
+
StopMode,
|
|
18
|
+
)
|
|
19
|
+
from ..tmc_logger import TmcLogger, Loglevel
|
|
20
|
+
from ..tmc_gpio import Gpio, GpioMode
|
|
21
|
+
from .. import tmc_gpio
|
|
22
|
+
from .. import _tmc_math as tmc_math
|
|
23
|
+
from .._tmc_exceptions import TmcMotionControlException
|
|
24
|
+
from ..platform_utils import get_time_us
|
|
25
|
+
|
|
26
|
+
|
|
27
|
+
class TmcMotionControlStepDir(TmcMotionControl):
|
|
28
|
+
"""STEP/DIR Motion Control class"""
|
|
29
|
+
|
|
30
|
+
@property
|
|
31
|
+
def max_speed(self):
|
|
32
|
+
"""_max_speed property"""
|
|
33
|
+
return self._max_speed
|
|
34
|
+
|
|
35
|
+
@max_speed.setter
|
|
36
|
+
def max_speed(self, speed: int):
|
|
37
|
+
"""_max_speed setter"""
|
|
38
|
+
speed = abs(speed)
|
|
39
|
+
if self._max_speed != speed:
|
|
40
|
+
self._max_speed = speed
|
|
41
|
+
if speed == 0.0:
|
|
42
|
+
self._cmin = 0
|
|
43
|
+
else:
|
|
44
|
+
self._cmin = round(1000000.0 / speed)
|
|
45
|
+
# Recompute _n from current speed and adjust speed if accelerating or cruising
|
|
46
|
+
if self._n > 0:
|
|
47
|
+
self._n = round(
|
|
48
|
+
(self._speed * self._speed) / (2.0 * self._acceleration)
|
|
49
|
+
) # Equation 16
|
|
50
|
+
self.compute_new_speed()
|
|
51
|
+
|
|
52
|
+
@property
|
|
53
|
+
def acceleration(self):
|
|
54
|
+
"""_acceleration property"""
|
|
55
|
+
return self._acceleration
|
|
56
|
+
|
|
57
|
+
@acceleration.setter
|
|
58
|
+
def acceleration(self, acceleration: int):
|
|
59
|
+
"""_acceleration setter"""
|
|
60
|
+
acceleration = abs(acceleration)
|
|
61
|
+
if acceleration == self._acceleration:
|
|
62
|
+
return
|
|
63
|
+
|
|
64
|
+
# Recompute _n per Equation 17
|
|
65
|
+
self._n = round(self._n * (self._acceleration / acceleration))
|
|
66
|
+
# New c0 per Equation 7, with correction per Equation 15
|
|
67
|
+
self._c0 = round(
|
|
68
|
+
0.676 * math.sqrt(2.0 / acceleration) * 1000000.0
|
|
69
|
+
) # Equation 15
|
|
70
|
+
self._acceleration = acceleration
|
|
71
|
+
self.compute_new_speed()
|
|
72
|
+
|
|
73
|
+
@property
|
|
74
|
+
def speed(self):
|
|
75
|
+
"""_speed property"""
|
|
76
|
+
return self._speed
|
|
77
|
+
|
|
78
|
+
@speed.setter
|
|
79
|
+
def speed(self, speed: int):
|
|
80
|
+
"""_speed setter"""
|
|
81
|
+
if speed == self._speed:
|
|
82
|
+
return
|
|
83
|
+
speed = tmc_math.constrain(speed, -self._max_speed, self._max_speed)
|
|
84
|
+
if speed == 0.0:
|
|
85
|
+
self._step_interval = 0
|
|
86
|
+
else:
|
|
87
|
+
self._step_interval = round(abs(1000000.0 / speed))
|
|
88
|
+
self.set_direction(Direction.CW if speed > 0 else Direction.CCW)
|
|
89
|
+
|
|
90
|
+
self._speed = speed
|
|
91
|
+
|
|
92
|
+
@property
|
|
93
|
+
def speed_fullstep(self):
|
|
94
|
+
"""_speed property"""
|
|
95
|
+
return self._speed * self.mres
|
|
96
|
+
|
|
97
|
+
@speed_fullstep.setter
|
|
98
|
+
def speed_fullstep(self, speed: int):
|
|
99
|
+
"""_speed setter"""
|
|
100
|
+
self.speed = speed * self.mres
|
|
101
|
+
|
|
102
|
+
@property
|
|
103
|
+
def pin_step(self):
|
|
104
|
+
"""_pin_step property"""
|
|
105
|
+
return self._pin_step
|
|
106
|
+
|
|
107
|
+
@property
|
|
108
|
+
def pin_dir(self):
|
|
109
|
+
"""_pin_dir property"""
|
|
110
|
+
return self._pin_dir
|
|
111
|
+
|
|
112
|
+
def __init__(self, pin_step: int, pin_dir: int | None):
|
|
113
|
+
"""constructor"""
|
|
114
|
+
super().__init__()
|
|
115
|
+
self._pin_step = pin_step
|
|
116
|
+
self._pin_dir = pin_dir
|
|
117
|
+
|
|
118
|
+
self._movement_thread: threading.Thread | None = None
|
|
119
|
+
|
|
120
|
+
self._sqrt_twoa: float = 1.0 # Precomputed sqrt(2*_acceleration)
|
|
121
|
+
self._step_interval: int = 0 # the current interval between two steps
|
|
122
|
+
self._min_pulse_width: int = 1 # minimum allowed pulse with in microseconds
|
|
123
|
+
self._last_step_time: int = 0 # The last step time in microseconds
|
|
124
|
+
self._n: int = 0 # step counter
|
|
125
|
+
self._c0: int = 0 # Initial step size in microseconds
|
|
126
|
+
self._cn: int = 0 # Last step size in microseconds
|
|
127
|
+
self._cmin: int = 0 # Min step size in microseconds based on maxSpeed
|
|
128
|
+
|
|
129
|
+
def init(self, tmc_logger: TmcLogger):
|
|
130
|
+
"""init: called by the Tmc class"""
|
|
131
|
+
super().init(tmc_logger)
|
|
132
|
+
self._tmc_logger.log(f"STEP Pin: {self._pin_step}", Loglevel.DEBUG)
|
|
133
|
+
tmc_gpio.tmc_gpio.gpio_setup(self._pin_step, GpioMode.OUT, initial=Gpio.LOW)
|
|
134
|
+
|
|
135
|
+
if self._pin_dir is not None:
|
|
136
|
+
self._tmc_logger.log(f"DIR Pin: {self._pin_dir}", Loglevel.DEBUG)
|
|
137
|
+
tmc_gpio.tmc_gpio.gpio_setup(
|
|
138
|
+
self._pin_dir, GpioMode.OUT, initial=int(self._direction)
|
|
139
|
+
)
|
|
140
|
+
|
|
141
|
+
def deinit(self):
|
|
142
|
+
"""destructor"""
|
|
143
|
+
if hasattr(self, "_pin_step"):
|
|
144
|
+
tmc_gpio.tmc_gpio.gpio_cleanup(self._pin_step)
|
|
145
|
+
del self._pin_step
|
|
146
|
+
if hasattr(self, "_pin_dir") and self._pin_dir is not None:
|
|
147
|
+
tmc_gpio.tmc_gpio.gpio_cleanup(self._pin_dir)
|
|
148
|
+
del self._pin_dir
|
|
149
|
+
|
|
150
|
+
def make_a_step(self):
|
|
151
|
+
"""method that makes on step
|
|
152
|
+
|
|
153
|
+
for the TMC2209 there needs to be a signal duration of minimum 100 ns
|
|
154
|
+
"""
|
|
155
|
+
tmc_gpio.tmc_gpio.gpio_output(self._pin_step, Gpio.HIGH)
|
|
156
|
+
time.sleep(1 / 1000 / 1000)
|
|
157
|
+
tmc_gpio.tmc_gpio.gpio_output(self._pin_step, Gpio.LOW)
|
|
158
|
+
time.sleep(1 / 1000 / 1000)
|
|
159
|
+
|
|
160
|
+
# self._tmc_logger.log("one step", Loglevel.MOVEMENT)
|
|
161
|
+
self._tmc_logger.log(
|
|
162
|
+
f"one step | cur: {self.current_pos} | tar: {self._target_pos}",
|
|
163
|
+
Loglevel.MOVEMENT,
|
|
164
|
+
)
|
|
165
|
+
|
|
166
|
+
def set_direction(self, direction: Direction):
|
|
167
|
+
"""sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
|
|
168
|
+
|
|
169
|
+
Args:
|
|
170
|
+
direction (bool): motor shaft direction: False = CCW; True = CW
|
|
171
|
+
"""
|
|
172
|
+
if self._pin_dir is None:
|
|
173
|
+
return
|
|
174
|
+
super().set_direction(direction)
|
|
175
|
+
tmc_gpio.tmc_gpio.gpio_output(self._pin_dir, int(direction))
|
|
176
|
+
|
|
177
|
+
def run_to_position_steps(
|
|
178
|
+
self, steps, movement_abs_rel: MovementAbsRel | None = None
|
|
179
|
+
) -> StopMode:
|
|
180
|
+
"""runs the motor to the given position.
|
|
181
|
+
with acceleration and deceleration
|
|
182
|
+
blocks the code until finished or stopped from a different thread!
|
|
183
|
+
returns true when the movement if finished normally and false,
|
|
184
|
+
when the movement was stopped
|
|
185
|
+
|
|
186
|
+
Args:
|
|
187
|
+
steps (int): amount of steps; can be negative
|
|
188
|
+
movement_abs_rel (enum): whether the movement should be absolut or relative
|
|
189
|
+
(Default value = None)
|
|
190
|
+
|
|
191
|
+
Returns:
|
|
192
|
+
stop (enum): how the movement was finished
|
|
193
|
+
"""
|
|
194
|
+
if movement_abs_rel is None:
|
|
195
|
+
movement_abs_rel = self._movement_abs_rel
|
|
196
|
+
|
|
197
|
+
if movement_abs_rel == MovementAbsRel.RELATIVE:
|
|
198
|
+
self._target_pos = self._current_pos + steps
|
|
199
|
+
else:
|
|
200
|
+
self._target_pos = steps
|
|
201
|
+
|
|
202
|
+
self._tmc_logger.log(
|
|
203
|
+
f"cur: {self._current_pos} | tar: {self._target_pos}", Loglevel.MOVEMENT
|
|
204
|
+
)
|
|
205
|
+
# self._tmc_logger.log(f"mov: {movement_abs_rel}", Loglevel.MOVEMENT)
|
|
206
|
+
|
|
207
|
+
self._stop = StopMode.NO
|
|
208
|
+
self._step_interval = 0
|
|
209
|
+
self._speed = 0.0
|
|
210
|
+
self._n = 0
|
|
211
|
+
self.compute_new_speed()
|
|
212
|
+
while self.run(): # returns false, when target position is reached
|
|
213
|
+
if self._stop == StopMode.HARDSTOP:
|
|
214
|
+
break
|
|
215
|
+
|
|
216
|
+
self._movement_phase = MovementPhase.STANDSTILL
|
|
217
|
+
return self._stop
|
|
218
|
+
|
|
219
|
+
def run_to_position_revolutions(
|
|
220
|
+
self, revolutions, movement_abs_rel: MovementAbsRel | None = None
|
|
221
|
+
) -> StopMode:
|
|
222
|
+
"""runs the motor to the given position.
|
|
223
|
+
with acceleration and deceleration
|
|
224
|
+
blocks the code until finished!
|
|
225
|
+
|
|
226
|
+
Args:
|
|
227
|
+
revolutions (int): amount of revs; can be negative
|
|
228
|
+
movement_abs_rel (enum): whether the movement should be absolut or relative
|
|
229
|
+
|
|
230
|
+
Returns:
|
|
231
|
+
stop (enum): how the movement was finished
|
|
232
|
+
"""
|
|
233
|
+
return self.run_to_position_steps(
|
|
234
|
+
round(revolutions * self._steps_per_rev), movement_abs_rel
|
|
235
|
+
)
|
|
236
|
+
|
|
237
|
+
def run_to_position_steps_threaded(
|
|
238
|
+
self, steps, movement_abs_rel: MovementAbsRel | None = None
|
|
239
|
+
):
|
|
240
|
+
"""runs the motor to the given position.
|
|
241
|
+
with acceleration and deceleration
|
|
242
|
+
does not block the code
|
|
243
|
+
returns true when the movement if finished normally and false,
|
|
244
|
+
when the movement was stopped
|
|
245
|
+
|
|
246
|
+
Args:
|
|
247
|
+
steps (int): amount of steps; can be negative
|
|
248
|
+
movement_abs_rel (enum): whether the movement should be absolut or relative
|
|
249
|
+
(Default value = None)
|
|
250
|
+
|
|
251
|
+
Returns:
|
|
252
|
+
stop (enum): how the movement was finished
|
|
253
|
+
"""
|
|
254
|
+
self._movement_thread = threading.Thread(
|
|
255
|
+
target=self.run_to_position_steps, args=(steps, movement_abs_rel)
|
|
256
|
+
)
|
|
257
|
+
self._movement_thread.start()
|
|
258
|
+
|
|
259
|
+
def run_to_position_revolutions_threaded(
|
|
260
|
+
self, revolutions, movement_abs_rel: MovementAbsRel | None = None
|
|
261
|
+
):
|
|
262
|
+
"""runs the motor to the given position.
|
|
263
|
+
with acceleration and deceleration
|
|
264
|
+
does not block the code
|
|
265
|
+
|
|
266
|
+
Args:
|
|
267
|
+
revolutions (int): amount of revs; can be negative
|
|
268
|
+
movement_abs_rel (enum): whether the movement should be absolut or relative
|
|
269
|
+
(Default value = None)
|
|
270
|
+
|
|
271
|
+
Returns:
|
|
272
|
+
stop (enum): how the movement was finished
|
|
273
|
+
"""
|
|
274
|
+
return self.run_to_position_steps_threaded(
|
|
275
|
+
round(revolutions * self._steps_per_rev), movement_abs_rel
|
|
276
|
+
)
|
|
277
|
+
|
|
278
|
+
def wait_for_movement_finished_threaded(self):
|
|
279
|
+
"""wait for the motor to finish the movement,
|
|
280
|
+
if started threaded
|
|
281
|
+
returns true when the movement if finished normally and false,
|
|
282
|
+
when the movement was stopped
|
|
283
|
+
|
|
284
|
+
Returns:
|
|
285
|
+
enum: how the movement was finished
|
|
286
|
+
"""
|
|
287
|
+
if self._movement_thread is None:
|
|
288
|
+
raise TmcMotionControlException("No movement thread running.")
|
|
289
|
+
self._movement_thread.join()
|
|
290
|
+
return self._stop
|
|
291
|
+
|
|
292
|
+
def run(self):
|
|
293
|
+
"""calculates a new speed if a speed was made
|
|
294
|
+
|
|
295
|
+
returns true if the target position is reached
|
|
296
|
+
should not be called from outside!
|
|
297
|
+
"""
|
|
298
|
+
if self.run_speed(): # returns true, when a step is made
|
|
299
|
+
self.compute_new_speed()
|
|
300
|
+
return self._speed != 0.0 and self.distance_to_go() != 0
|
|
301
|
+
|
|
302
|
+
def distance_to_go(self):
|
|
303
|
+
"""returns the remaining distance the motor should run"""
|
|
304
|
+
distance_to_go = self._target_pos - self._current_pos
|
|
305
|
+
# self._tmc_logger.log(f"cur: {self.current_pos} | tar: {self._target_pos} | dis: {distance_to_go}", Loglevel.MOVEMENT)
|
|
306
|
+
return distance_to_go
|
|
307
|
+
|
|
308
|
+
def compute_new_speed(self):
|
|
309
|
+
"""returns the calculated current speed depending on the acceleration
|
|
310
|
+
|
|
311
|
+
this code is based on:
|
|
312
|
+
"Generate stepper-motor speed profiles in real time" by David Austin
|
|
313
|
+
https://www.embedded.com/generate-stepper-motor-speed-profiles-in-real-time/
|
|
314
|
+
https://web.archive.org/web/20140705143928/http://fab.cba.mit.edu/classes/MIT/961.09/projects/i0/Stepper_Motor_Speed_Profile.pdf
|
|
315
|
+
"""
|
|
316
|
+
distance_to = self.distance_to_go() # +ve is clockwise from current location
|
|
317
|
+
steps_to_stop = round(
|
|
318
|
+
(self._speed * self._speed) / (2.0 * self._acceleration)
|
|
319
|
+
) # Equation 16
|
|
320
|
+
if (distance_to == 0 and steps_to_stop <= 2) or (
|
|
321
|
+
self._stop == StopMode.SOFTSTOP and steps_to_stop <= 1
|
|
322
|
+
):
|
|
323
|
+
# We are at the target and its time to stop
|
|
324
|
+
self._step_interval = 0
|
|
325
|
+
self._speed = 0.0
|
|
326
|
+
self._n = 0
|
|
327
|
+
self._movement_phase = MovementPhase.STANDSTILL
|
|
328
|
+
self._tmc_logger.log("time to stop", Loglevel.MOVEMENT)
|
|
329
|
+
return
|
|
330
|
+
|
|
331
|
+
if distance_to > 0:
|
|
332
|
+
# We are anticlockwise from the target
|
|
333
|
+
# Need to go clockwise from here, maybe decelerate now
|
|
334
|
+
if self._n > 0:
|
|
335
|
+
# Currently accelerating, need to decel now? Or maybe going the wrong way?
|
|
336
|
+
if (
|
|
337
|
+
(steps_to_stop >= distance_to)
|
|
338
|
+
or self._direction == Direction.CCW
|
|
339
|
+
or self._stop == StopMode.SOFTSTOP
|
|
340
|
+
):
|
|
341
|
+
self._n = -steps_to_stop # Start deceleration
|
|
342
|
+
self._movement_phase = MovementPhase.DECELERATING
|
|
343
|
+
elif self._n < 0:
|
|
344
|
+
# Currently decelerating, need to accel again?
|
|
345
|
+
if (steps_to_stop < distance_to) and self._direction == Direction.CW:
|
|
346
|
+
self._n = -self._n # Start acceleration
|
|
347
|
+
self._movement_phase = MovementPhase.ACCELERATING
|
|
348
|
+
elif distance_to < 0:
|
|
349
|
+
# We are clockwise from the target
|
|
350
|
+
# Need to go anticlockwise from here, maybe decelerate
|
|
351
|
+
if self._n > 0:
|
|
352
|
+
# Currently accelerating, need to decel now? Or maybe going the wrong way?
|
|
353
|
+
if (
|
|
354
|
+
(steps_to_stop >= -distance_to)
|
|
355
|
+
or self._direction == Direction.CW
|
|
356
|
+
or self._stop == StopMode.SOFTSTOP
|
|
357
|
+
):
|
|
358
|
+
self._n = -steps_to_stop # Start deceleration
|
|
359
|
+
self._movement_phase = MovementPhase.DECELERATING
|
|
360
|
+
elif self._n < 0:
|
|
361
|
+
# Currently decelerating, need to accel again?
|
|
362
|
+
if (steps_to_stop < -distance_to) and self._direction == Direction.CCW:
|
|
363
|
+
self._n = -self._n # Start acceleration
|
|
364
|
+
self._movement_phase = MovementPhase.ACCELERATING
|
|
365
|
+
# Need to accelerate or decelerate
|
|
366
|
+
if self._n == 0:
|
|
367
|
+
# First step from stopped
|
|
368
|
+
self._cn = self._c0
|
|
369
|
+
tmc_gpio.tmc_gpio.gpio_output(self._pin_step, Gpio.LOW)
|
|
370
|
+
if distance_to > 0:
|
|
371
|
+
self.set_direction(Direction.CW)
|
|
372
|
+
self._tmc_logger.log("going CW", Loglevel.MOVEMENT)
|
|
373
|
+
else:
|
|
374
|
+
self.set_direction(Direction.CCW)
|
|
375
|
+
self._movement_phase = MovementPhase.ACCELERATING
|
|
376
|
+
else:
|
|
377
|
+
# Subsequent step. Works for accel (n is +_ve) and decel (n is -ve).
|
|
378
|
+
self._cn = round(
|
|
379
|
+
self._cn - ((2.0 * self._cn) / ((4.0 * self._n) + 1))
|
|
380
|
+
) # Equation 13
|
|
381
|
+
self._cn = max(self._cn, self._cmin)
|
|
382
|
+
if self._cn == self._cmin:
|
|
383
|
+
self._movement_phase = MovementPhase.MAXSPEED
|
|
384
|
+
self._n += 1
|
|
385
|
+
self._step_interval = self._cn
|
|
386
|
+
self._speed = 1000000.0 / self._cn
|
|
387
|
+
if self._direction == Direction.CCW:
|
|
388
|
+
self._speed = -self._speed
|
|
389
|
+
|
|
390
|
+
def run_speed(self):
|
|
391
|
+
"""this methods does the actual steps with the current speed"""
|
|
392
|
+
# Don't do anything unless we actually have a step interval
|
|
393
|
+
|
|
394
|
+
# self._tmc_logger.log(f"si: {self._step_interval}")
|
|
395
|
+
|
|
396
|
+
if not self._step_interval:
|
|
397
|
+
return False
|
|
398
|
+
|
|
399
|
+
curtime = get_time_us()
|
|
400
|
+
|
|
401
|
+
if curtime - self._last_step_time >= self._step_interval:
|
|
402
|
+
|
|
403
|
+
self._tmc_logger.log(f"dir: {self._direction}", Loglevel.MOVEMENT)
|
|
404
|
+
|
|
405
|
+
if self._direction == Direction.CW: # Clockwise
|
|
406
|
+
self._current_pos += 1
|
|
407
|
+
else: # Anticlockwise
|
|
408
|
+
self._current_pos -= 1
|
|
409
|
+
self.make_a_step()
|
|
410
|
+
|
|
411
|
+
self._last_step_time = (
|
|
412
|
+
curtime # Caution: does not account for costs in step()
|
|
413
|
+
)
|
|
414
|
+
return True
|
|
415
|
+
return False
|
|
@@ -0,0 +1,78 @@
|
|
|
1
|
+
# pylint: disable=too-many-instance-attributes
|
|
2
|
+
# pylint: disable=too-many-arguments
|
|
3
|
+
# pylint: disable=too-many-branches
|
|
4
|
+
# pylint: disable=too-many-positional-arguments
|
|
5
|
+
"""
|
|
6
|
+
STEP_PWM/DIR Motion Control module
|
|
7
|
+
"""
|
|
8
|
+
|
|
9
|
+
from ._tmc_mc import Direction, StopMode
|
|
10
|
+
from ._tmc_mc_step_dir import TmcMotionControlStepDir
|
|
11
|
+
from ..tmc_logger import TmcLogger, Loglevel
|
|
12
|
+
from .. import tmc_gpio
|
|
13
|
+
from .._tmc_exceptions import TmcMotionControlException
|
|
14
|
+
|
|
15
|
+
|
|
16
|
+
class TmcMotionControlStepPwmDir(TmcMotionControlStepDir):
|
|
17
|
+
"""STEP_PWM/DIR Motion Control class"""
|
|
18
|
+
|
|
19
|
+
@property
|
|
20
|
+
def speed(self):
|
|
21
|
+
"""_speed property"""
|
|
22
|
+
return self._speed
|
|
23
|
+
|
|
24
|
+
@speed.setter
|
|
25
|
+
def speed(self, speed: int):
|
|
26
|
+
"""_speed setter"""
|
|
27
|
+
self._speed = speed
|
|
28
|
+
tmc_gpio.tmc_gpio.gpio_pwm_set_frequency(self._pin_step, self._speed)
|
|
29
|
+
self._tmc_logger.log(f"Speed: {self._speed} msteps/s", Loglevel.DEBUG)
|
|
30
|
+
|
|
31
|
+
def init(self, tmc_logger: TmcLogger):
|
|
32
|
+
"""init: called by the Tmc class"""
|
|
33
|
+
super().init(tmc_logger)
|
|
34
|
+
tmc_gpio.tmc_gpio.gpio_pwm_setup(self._pin_step, 1, 0)
|
|
35
|
+
|
|
36
|
+
def stop(self, stop_mode=StopMode.HARDSTOP):
|
|
37
|
+
"""stop the current movement
|
|
38
|
+
|
|
39
|
+
Args:
|
|
40
|
+
stop_mode (enum): whether the movement should be stopped immediately or softly
|
|
41
|
+
(Default value = StopMode.HARDSTOP)
|
|
42
|
+
"""
|
|
43
|
+
super().stop(stop_mode)
|
|
44
|
+
tmc_gpio.tmc_gpio.gpio_pwm_set_duty_cycle(self._pin_step, 0)
|
|
45
|
+
|
|
46
|
+
def run_speed_pwm(self, speed: int | None = None):
|
|
47
|
+
"""runs the motor
|
|
48
|
+
does not block the code
|
|
49
|
+
"""
|
|
50
|
+
if self._pin_step is None:
|
|
51
|
+
raise TmcMotionControlException(
|
|
52
|
+
"Step pin not set for STEP_PWM/DIR motion control"
|
|
53
|
+
)
|
|
54
|
+
|
|
55
|
+
if speed is None:
|
|
56
|
+
speed = self.max_speed
|
|
57
|
+
|
|
58
|
+
if speed == 0:
|
|
59
|
+
# stop movement
|
|
60
|
+
tmc_gpio.tmc_gpio.gpio_pwm_set_duty_cycle(self._pin_step, 0)
|
|
61
|
+
else:
|
|
62
|
+
if speed < 0:
|
|
63
|
+
self.set_direction(Direction.CCW)
|
|
64
|
+
speed = -speed
|
|
65
|
+
else:
|
|
66
|
+
self.set_direction(Direction.CW)
|
|
67
|
+
|
|
68
|
+
# change pwm frequency
|
|
69
|
+
self.speed = speed
|
|
70
|
+
tmc_gpio.tmc_gpio.gpio_pwm_set_duty_cycle(self._pin_step, 50)
|
|
71
|
+
|
|
72
|
+
def run_speed_pwm_fullstep(self, speed: int | None = None):
|
|
73
|
+
"""runs the motor
|
|
74
|
+
does not block the code
|
|
75
|
+
"""
|
|
76
|
+
if speed is None:
|
|
77
|
+
speed = self.max_speed_fullstep
|
|
78
|
+
self.run_speed_pwm(speed * self.mres)
|
|
@@ -0,0 +1,36 @@
|
|
|
1
|
+
"""
|
|
2
|
+
STEP/REG Motion Control module
|
|
3
|
+
"""
|
|
4
|
+
|
|
5
|
+
from ._tmc_mc import Direction
|
|
6
|
+
from ._tmc_mc_step_dir import TmcMotionControlStepDir
|
|
7
|
+
from ..tmc_logger import Loglevel
|
|
8
|
+
from .. import tmc_gpio
|
|
9
|
+
from ..reg import _tmc_shared_regs as tmc_shared_reg
|
|
10
|
+
|
|
11
|
+
|
|
12
|
+
class TmcMotionControlStepReg(TmcMotionControlStepDir):
|
|
13
|
+
"""STEP/REG Motion Control class"""
|
|
14
|
+
|
|
15
|
+
def __init__(self, pin_step: int):
|
|
16
|
+
"""constructor"""
|
|
17
|
+
super().__init__(pin_step, None)
|
|
18
|
+
|
|
19
|
+
def deinit(self):
|
|
20
|
+
"""destructor"""
|
|
21
|
+
if self._pin_step is not None:
|
|
22
|
+
tmc_gpio.tmc_gpio.gpio_cleanup(self._pin_step)
|
|
23
|
+
self._pin_step = None
|
|
24
|
+
|
|
25
|
+
def set_direction(self, direction: Direction):
|
|
26
|
+
"""sets the motor shaft direction to the given value: 0 = CCW; 1 = CW
|
|
27
|
+
|
|
28
|
+
Args:
|
|
29
|
+
direction (bool): motor shaft direction: False = CCW; True = CW
|
|
30
|
+
"""
|
|
31
|
+
self._direction = direction
|
|
32
|
+
self._tmc_logger.log(f"New Direction is: {direction}", Loglevel.MOVEMENT)
|
|
33
|
+
|
|
34
|
+
gconf: tmc_shared_reg.GConf = self.get_register("gconf")
|
|
35
|
+
|
|
36
|
+
gconf.modify("shaft", bool(int(direction)))
|