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