grblHAL core  20261015
stepper2.h
Go to the documentation of this file.
1 /*
2  stepper2.h - secondary stepper motor driver
3 
4  Part of grblHAL
5 
6  Copyright (c) 2023 - 2026 Terje Io
7 
8  grblHAL is free software: you can redistribute it and/or modify
9  it under the terms of the GNU General Public License as published by
10  the Free Software Foundation, either version 3 of the License, or
11  (at your option) any later version.
12 
13  grblHAL is distributed in the hope that it will be useful,
14  but WITHOUT ANY WARRANTY; without even the implied warranty of
15  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  GNU General Public License for more details.
17 
18  You should have received a copy of the GNU General Public License
19  along with grblHAL. If not, see <http://www.gnu.org/licenses/>.
20 */
21 
22 #include <stdint.h>
23 #include <stdbool.h>
24 
25 #include "task.h"
26 
27 typedef enum {
32 
33 struct st2_motor; // members defined in stepper2.c
34 typedef struct st2_motor st2_motor_t;
35 
36 st2_motor_t *st2_motor_init (uint_fast8_t axis_idx, bool is_spindle);
37 bool st2_motor_poll (st2_motor_t *motor);
38 bool st2_motor_bind_spindle (uint_fast8_t axis_idx, axis_settings_t *cfg);
39 float st2_get_speed (st2_motor_t *motor);
40 float st2_motor_set_speed (st2_motor_t *motor, float speed);
41 bool st2_motor_move (st2_motor_t *motor, const float move, const float speed, position_t type);
42 bool st2_motor_run (st2_motor_t *motor);
43 bool st2_motor_running (st2_motor_t *motor);
44 bool st2_motor_cruising (st2_motor_t *motor);
45 bool st2_motor_stop (st2_motor_t *motor);
46 int64_t st2_get_position (st2_motor_t *motor);
47 bool st2_set_position (st2_motor_t *motor, int64_t position);
49 
50 // Lowlevel API
51 
52 typedef enum {
53  State_Idle = 0,
60 
61 typedef struct {
62  position_t ptype; // finite steps, distance or infinite motion
63  st2_state_t state; // state machine state
64  uint32_t move; // total steps to move
65  uint32_t step_no; // ramp cursor, may be changed by controlled stop
66  uint32_t step_run; // end of acceleration or speed transition
67  uint32_t step_down; // start of down-ramp
68  uint64_t c64; // 24.16 fixed point delay count
69  uint64_t delay; // microseconds until the next service
70  uint32_t step_length; // step pulse length, includes min. off period
71  uint32_t first_delay; // integer delay count
72  uint32_t min_delay; // integer delay count
73  int32_t denom; // 4.n+1 in ramp algo
74  uint32_t n; // accel/decel steps
75  float speed; // speed steps/s
76  float prev_speed; // speed steps/s
77  float steps_per_mm; // unit conversion, steps/mm
78  float acceleration; // acceleration steps/s^2
80 
81 __attribute__((always_inline)) static inline void _st2_config (st2_profile_t *profile, axis_settings_t *cfg)
82 {
83  profile->steps_per_mm = cfg->steps_per_mm;
84  profile->acceleration = cfg->acceleration * cfg->steps_per_mm / 3600.0f;
85  profile->step_length = (uint32_t)ceilf(settings.steppers.pulse_microseconds + 2.0f);
86  if((profile->first_delay = (uint32_t)(0.676f * sqrtf(2.0f / profile->acceleration) * 1000000.0f)) < profile->step_length)
87  profile->first_delay = profile->step_length;
88 }
89 
90 __attribute__((always_inline)) static inline float _st2_get_speed (st2_profile_t *profile)
91 {
92  return profile->state == State_Idle ? 0.0f : 60.0f / ((float)profile->delay * profile->steps_per_mm / 1000000.0f);
93 }
94 
95 __attribute__((always_inline)) static inline float _st2_set_speed (st2_profile_t *profile, float steps_per_second)
96 {
97  if((profile->speed = steps_per_second) == profile->prev_speed)
98  return profile->speed;
99 
100  profile->n = (uint32_t)((profile->speed * profile->speed) / (2.0f * profile->acceleration));
101  if((profile->min_delay = (uint32_t)(1000000.0f / profile->speed)) < profile->step_length)
102  profile->min_delay = profile->step_length;
103 
104  if(profile->n == 0)
105  profile->n = 1;
106 
107  if(profile->state != State_Idle) {
108 
109  int32_t pn = profile->n - ((profile->denom - 1) >> 2);
110 
111  if(pn == 0)
112  return profile->speed;
113 
114 #if ST2_DEBUG
115  debug_printf("!!: %d %.2f %.3f %d %d %d %d", profile->state, profile->prev_speed, profile->speed, profile->denom - 1, profile->n, pn, profile->denom);
116 #endif
117 
118  if(profile->speed > profile->prev_speed) {
119  if(profile->state == State_Accel)
120  profile->step_run += pn;
121  else {
122  profile->step_run = profile->step_no + pn;
123  profile->state = State_Accel;
124  }
125  } else {
126  if(profile->speed == 0.0f)
127  profile->state = State_Decel;
128  if(profile->state != State_Decel) {
129  profile->step_run = profile->step_no - pn;
130  profile->state = State_DecelTo;
131  }
132  }
133  }
134 
135  profile->prev_speed = profile->speed;
136 
137  if(profile->first_delay < profile->min_delay)
138  profile->first_delay = profile->min_delay;
139 
140  return profile->prev_speed;
141 }
142 
143 __attribute__((always_inline)) static inline uint32_t _st2_calc_steps (st2_profile_t *profile, float move, position_t ptype)
144 {
145 // TODO: keep track of fractional steps?
146 
147  switch((profile->ptype = ptype)) {
148 
149  case Stepper2_Steps:
151  profile->move = (uint32_t)fabsf(move);
152  break;
153 
154  case Stepper2_mm:
155  profile->move = (uint32_t)lroundf(fabsf(move * profile->steps_per_mm));
156  break;
157  }
158 
159  return profile->move;
160 }
161 
162 __attribute__((always_inline)) static inline bool _st2_start (st2_profile_t *profile)
163 {
164  if(profile->ptype == Stepper2_InfiniteSteps) {
165  profile->step_run = profile->n;
166  profile->step_down = profile->n + 1;
167  } else if(profile->move != 0) {
168  profile->step_run = (profile->move - ((profile->move & 0x0001) ? 1 : 0)) >> 1;
169  if(profile->step_run > profile->n)
170  profile->step_run = profile->n;
171  profile->step_down = profile->move - profile->step_run;
172  } else
173  return false;
174 
175  profile->state = State_Accel;
176  profile->delay = profile->first_delay;
177  profile->c64 = profile->delay << 16; // keep delay in 24.16 fixed-point format for ramp calcs
178  profile->denom = 1; // 4.n + 1, n = 0
179  profile->step_no = 0; // ramp cursor
180 
181  return true;
182 }
183 
184 __attribute__((always_inline)) static inline bool _st2_run (st2_profile_t *profile)
185 {
186  switch(profile->state) {
187 
188  case State_Accel:
189  if(profile->step_no != profile->step_run) {
190  profile->denom += 4;
191  profile->c64 -= (profile->c64 << 1) / profile->denom; // ramp algorithm
192  profile->delay = (profile->c64 + 32768) >> 16; // round 24.16 format -> int16
193  if(profile->delay < profile->min_delay) { // go to constant speed?
194  // profile->denom -= 6; // causes issues with speed override for infinite moves
195  profile->state = profile->ptype == Stepper2_InfiniteSteps ? State_RunInfinite : State_Run;
196  profile->step_down = profile->move - (profile->step_no + 1);
197  profile->delay = profile->min_delay;
198  }
199  break;
200  } else if((profile->state = profile->step_run == profile->step_down ? State_Decel : (profile->ptype == Stepper2_InfiniteSteps ? State_RunInfinite : State_Run)) != State_Decel) {
201  profile->delay = profile->min_delay;
202  break;
203  } // else
204  // no break
205 
206  case State_Run:
207  if(profile->step_no != profile->step_down)
208  break;
209  else
210  profile->state = State_Decel;
211  // no break
212 
213  case State_Decel:
214  if(profile->denom < 2) { // done?
215  profile->state = State_Idle;
216  profile->prev_speed = 0.0f;
217  profile->n = 0;
218 #if ST2_DEBUG
219  debug_writeln(uitoa(profile->position));
220 #endif
221  } else {
222  profile->c64 += (profile->c64 << 1) / profile->denom; // ramp algorithm
223  profile->delay = (profile->c64 - 32768) >> 16; // round 24.16 format -> int16
224  profile->denom -= 4;
225  }
226  break;
227 
228  case State_DecelTo:
229  if(profile->step_no != profile->step_run) {
230  profile->denom -= 4;
231  profile->c64 += (profile->c64 << 1) / profile->denom; // ramp algorithm
232  profile->delay = (profile->c64 + 32768) >> 16; // round 24.16 format -> int16
233  } else {
234  profile->delay = profile->min_delay;
235  profile->state = profile->ptype == Stepper2_InfiniteSteps ? State_RunInfinite : State_Run;
236  }
237  break;
238 
239  default:
240  break;
241  }
242 
243  if(profile->state != State_Idle)
244  profile->step_no++;
245 
246  return profile->state != State_Idle;
247 }
248 
254 __attribute__((always_inline)) static inline bool _st2_stop (st2_profile_t *profile)
255 {
256  switch(profile->state) {
257 
258  case State_Accel:
259  profile->step_no = profile->step_down - 1;
260  profile->step_run = profile->step_down;
261  break;
262 
263  case State_Run:
264  profile->step_no = profile->step_down - 1;
265  break;
266 
267  case State_RunInfinite:
268  case State_DecelTo:
269  profile->state = State_Decel;
270  break;
271 
272  default:
273  break;
274  }
275 
276  return profile->state != State_Idle;
277 }
278 
283 __attribute__((always_inline)) static inline bool _st2_running (st2_profile_t *profile)
284 {
285  return profile->state != State_Idle;
286 }
287 
292 __attribute__((always_inline)) static inline bool _st2_cruising (st2_profile_t *profile)
293 {
294  return profile->state == State_Run || profile->state == State_RunInfinite;
295 }
char * uitoa(uint32_t n)
Definition: nuts_bolts.c:107
settings_t settings
Definition: settings.c:48
int64_t st2_get_position(st2_motor_t *motor)
Get current position in steps.
Definition: stepper2.c:391
bool st2_motor_bind_spindle(uint_fast8_t axis_idx, axis_settings_t *cfg)
Bind and initialize a motor.
Definition: stepper2.c:210
bool st2_motor_move(st2_motor_t *motor, const float move, const float speed, position_t type)
Command a motor to move.
Definition: stepper2.c:345
float st2_motor_set_speed(st2_motor_t *motor, float speed)
Set speed.
Definition: stepper2.c:320
position_t
Definition: stepper2.h:27
@ Stepper2_Steps
0
Definition: stepper2.h:28
@ Stepper2_mm
2
Definition: stepper2.h:30
@ Stepper2_InfiniteSteps
1
Definition: stepper2.h:29
st2_motor_t * st2_motor_init(uint_fast8_t axis_idx, bool is_spindle)
Bind and initialize a motor.
Definition: stepper2.c:255
bool st2_set_position(st2_motor_t *motor, int64_t position)
Set current position in steps.
Definition: stepper2.c:403
void st2_motor_register_stopped_callback(st2_motor_t *motor, foreground_task_ptr callback)
Definition: stepper2.c:199
float st2_get_speed(st2_motor_t *motor)
Get current speed (RPM).
Definition: stepper2.c:307
bool st2_motor_run(st2_motor_t *motor)
Execute a move commanded by st2_motor_move().
Definition: stepper2.c:450
bool st2_motor_cruising(st2_motor_t *motor)
Check if motor is running in cruising phase.
Definition: stepper2.c:499
bool st2_motor_poll(st2_motor_t *motor)
Check if motor is run by polling.
Definition: stepper2.c:481
bool st2_motor_running(st2_motor_t *motor)
Check if motor is running.
Definition: stepper2.c:490
bool st2_motor_stop(st2_motor_t *motor)
Stop a move. This will initiate deceleration to stop the motor if it is running.
Definition: stepper2.c:472
st2_state_t
Definition: stepper2.h:52
@ State_DecelTo
4
Definition: stepper2.h:57
@ State_Run
2
Definition: stepper2.h:55
@ State_Accel
1
Definition: stepper2.h:54
@ State_Decel
5
Definition: stepper2.h:58
@ State_Idle
0
Definition: stepper2.h:53
@ State_RunInfinite
3
Definition: stepper2.h:56
FLASHMEM void debug_printf(const char *fmt,...)
Definition: stream.c:1070
Definition: settings.h:764
float acceleration
Definition: settings.h:767
float steps_per_mm
Definition: settings.h:765
stepper_settings_t steppers
Definition: settings.h:924
Internal structure for holding motor configuration and keeping track of its status.
Definition: stepper2.c:51
volatile int64_t position
Definition: stepper2.c:55
Definition: stepper2.h:61
uint32_t move
Definition: stepper2.h:64
uint32_t step_length
Definition: stepper2.h:70
uint32_t n
Definition: stepper2.h:74
uint32_t step_down
Definition: stepper2.h:67
uint32_t min_delay
Definition: stepper2.h:72
uint32_t step_no
Definition: stepper2.h:65
uint32_t step_run
Definition: stepper2.h:66
float speed
Definition: stepper2.h:75
uint64_t delay
Definition: stepper2.h:69
float prev_speed
Definition: stepper2.h:76
float acceleration
Definition: stepper2.h:78
int32_t denom
Definition: stepper2.h:73
uint64_t c64
Definition: stepper2.h:68
position_t ptype
Definition: stepper2.h:62
uint32_t first_delay
Definition: stepper2.h:71
st2_state_t state
Definition: stepper2.h:63
float steps_per_mm
Definition: stepper2.h:77
float pulse_microseconds
Definition: settings.h:759
void(* foreground_task_ptr)(void *data)
Definition: task.h:25