2 This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl) with additions from Sungeun K. Jeon (https://github.com/chamnit/grbl)
3 Smoothie is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version.
4 Smoothie is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details.
5 You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
10 #include "libs/Module.h"
11 #include "libs/Kernel.h"
14 #include "StepperMotor.h"
16 #include "checksumm.h"
17 #include "SlowTicker.h"
19 #include "ConfigValue.h"
22 #include "StepTicker.h"
27 #include "libs/nuts_bolts.h"
28 #include "libs/Hook.h"
32 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
33 // TODO: This does accel, accel should be in StepperMotor
37 this->current_block
= NULL
;
39 this->force_speed_update
= false;
43 //Called when the module has just been loaded
44 void Stepper::on_module_loaded()
46 this->register_for_event(ON_BLOCK_BEGIN
);
47 this->register_for_event(ON_BLOCK_END
);
48 this->register_for_event(ON_GCODE_EXECUTE
);
49 this->register_for_event(ON_GCODE_RECEIVED
);
50 this->register_for_event(ON_PLAY
);
51 this->register_for_event(ON_PAUSE
);
52 this->register_for_event(ON_HALT
);
55 this->on_config_reload(this);
57 // Acceleration ticker
58 THEKERNEL
->step_ticker
->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); });
60 // Attach to the end_of_move stepper event
61 THEKERNEL
->robot
->alpha_stepper_motor
->attach(this, &Stepper::stepper_motor_finished_move
);
62 THEKERNEL
->robot
->beta_stepper_motor
->attach( this, &Stepper::stepper_motor_finished_move
);
63 THEKERNEL
->robot
->gamma_stepper_motor
->attach(this, &Stepper::stepper_motor_finished_move
);
66 // Get configuration from the config file
67 void Stepper::on_config_reload(void *argument
)
69 // Steppers start off by default
70 this->turn_enable_pins_off();
73 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
74 void Stepper::on_pause(void *argument
)
77 THEKERNEL
->robot
->alpha_stepper_motor
->pause();
78 THEKERNEL
->robot
->beta_stepper_motor
->pause();
79 THEKERNEL
->robot
->gamma_stepper_motor
->pause();
82 // When the play/pause button is set to play, or a module calls the ON_PLAY event
83 void Stepper::on_play(void *argument
)
85 // TODO: Re-compute the whole queue for a cold-start
87 THEKERNEL
->robot
->alpha_stepper_motor
->unpause();
88 THEKERNEL
->robot
->beta_stepper_motor
->unpause();
89 THEKERNEL
->robot
->gamma_stepper_motor
->unpause();
92 void Stepper::on_halt(void *argument
)
94 if(argument
== nullptr) {
95 this->turn_enable_pins_off();
102 void Stepper::on_gcode_received(void *argument
)
104 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
105 // Attach gcodes to the last block for on_gcode_execute
106 if( gcode
->has_m
&& (gcode
->m
== 84 || gcode
->m
== 17 || gcode
->m
== 18 )) {
107 THEKERNEL
->conveyor
->append_gcode(gcode
);
111 // React to enable/disable gcodes
112 void Stepper::on_gcode_execute(void *argument
)
114 Gcode
*gcode
= static_cast<Gcode
*>(argument
);
117 if( gcode
->m
== 17 ) {
118 this->turn_enable_pins_on();
120 if( (gcode
->m
== 84 || gcode
->m
== 18) && !gcode
->has_letter('E') ) {
121 this->turn_enable_pins_off();
127 void Stepper::turn_enable_pins_on()
129 for (StepperMotor
*m
: THEKERNEL
->robot
->actuators
)
131 this->enable_pins_status
= true;
135 void Stepper::turn_enable_pins_off()
137 for (StepperMotor
*m
: THEKERNEL
->robot
->actuators
)
139 this->enable_pins_status
= false;
142 // A new block is popped from the queue
143 void Stepper::on_block_begin(void *argument
)
145 Block
*block
= static_cast<Block
*>(argument
);
147 // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
148 if(block
->millimeters
> 0.0F
&& (block
->steps
[ALPHA_STEPPER
] > 0 || block
->steps
[BETA_STEPPER
] > 0 || block
->steps
[GAMMA_STEPPER
] > 0) ) {
152 // none of the steppers move this block so make sure they know that
153 for(auto a
: THEKERNEL
->robot
->actuators
) {
154 a
->set_moved_last_block(false);
159 // We can't move with the enable pins off
160 if( this->enable_pins_status
== false ) {
161 this->turn_enable_pins_on();
164 // Setup : instruct stepper motors to move
165 // Find the stepper with the more steps, it's the one the speed calculations will want to follow
166 this->main_stepper
= nullptr;
167 if( block
->steps
[ALPHA_STEPPER
] > 0 ) {
168 THEKERNEL
->robot
->alpha_stepper_motor
->move( block
->direction_bits
[ALPHA_STEPPER
], block
->steps
[ALPHA_STEPPER
])->set_moved_last_block(true);
169 this->main_stepper
= THEKERNEL
->robot
->alpha_stepper_motor
;
171 THEKERNEL
->robot
->alpha_stepper_motor
->set_moved_last_block(false);
174 if( block
->steps
[BETA_STEPPER
] > 0 ) {
175 THEKERNEL
->robot
->beta_stepper_motor
->move( block
->direction_bits
[BETA_STEPPER
], block
->steps
[BETA_STEPPER
])->set_moved_last_block(true);
176 if(this->main_stepper
== nullptr || THEKERNEL
->robot
->beta_stepper_motor
->get_steps_to_move() > this->main_stepper
->get_steps_to_move())
177 this->main_stepper
= THEKERNEL
->robot
->beta_stepper_motor
;
179 THEKERNEL
->robot
->beta_stepper_motor
->set_moved_last_block(false);
182 if( block
->steps
[GAMMA_STEPPER
] > 0 ) {
183 THEKERNEL
->robot
->gamma_stepper_motor
->move( block
->direction_bits
[GAMMA_STEPPER
], block
->steps
[GAMMA_STEPPER
])->set_moved_last_block(true);
184 if(this->main_stepper
== nullptr || THEKERNEL
->robot
->gamma_stepper_motor
->get_steps_to_move() > this->main_stepper
->get_steps_to_move())
185 this->main_stepper
= THEKERNEL
->robot
->gamma_stepper_motor
;
187 THEKERNEL
->robot
->gamma_stepper_motor
->set_moved_last_block(false);
190 this->current_block
= block
;
192 // Setup acceleration for this block
193 this->trapezoid_generator_reset();
195 // Set the initial speed for this move
196 this->trapezoid_generator_tick();
198 // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block
199 THEKERNEL
->step_ticker
->synchronize_acceleration(false);
201 // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step
202 if( block
->decelerate_after
> 0 && block
->decelerate_after
+1 < this->main_stepper
->steps_to_move
) {
203 this->main_stepper
->signal_step
= block
->decelerate_after
+1; // we make it +1 as deceleration does not start until steps > decelerate_after
207 // Current block is discarded
208 void Stepper::on_block_end(void *argument
)
210 this->current_block
= NULL
; //stfu !
213 // When a stepper motor has finished it's assigned movement
214 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy
)
216 // We care only if none is still moving
217 if( THEKERNEL
->robot
->alpha_stepper_motor
->moving
|| THEKERNEL
->robot
->beta_stepper_motor
->moving
|| THEKERNEL
->robot
->gamma_stepper_motor
->moving
) {
221 // This block is finished, release it
222 if( this->current_block
!= NULL
) {
223 this->current_block
->release();
230 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
231 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
232 // current_block stays untouched by outside handlers for the duration of this function call.
233 // NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv
234 void Stepper::trapezoid_generator_tick(void)
236 // Do not do the accel math for nothing
237 if(this->current_block
&& !this->paused
&& this->main_stepper
->moving
) {
239 // Store this here because we use it a lot down there
240 uint32_t current_steps_completed
= this->main_stepper
->stepped
;
241 float last_rate
= trapezoid_adjusted_rate
;
243 if( this->force_speed_update
) {
244 // Do not accel, just set the value
245 this->force_speed_update
= false;
248 } else if(THEKERNEL
->conveyor
->is_flushing()) {
249 // if we are flushing the queue, decelerate to 0 then finish this block
250 if (trapezoid_adjusted_rate
> current_block
->rate_delta
* 1.5F
) {
251 trapezoid_adjusted_rate
-= current_block
->rate_delta
;
253 } else if (trapezoid_adjusted_rate
== current_block
->rate_delta
* 0.5F
) {
254 for (auto i
: THEKERNEL
->robot
->actuators
) i
->move(i
->direction
, 0); // stop motors
255 if (current_block
) current_block
->release();
256 THEKERNEL
->call_event(ON_SPEED_CHANGE
, 0); // tell others we stopped
260 trapezoid_adjusted_rate
= current_block
->rate_delta
* 0.5F
;
263 } else if(current_steps_completed
<= this->current_block
->accelerate_until
) {
264 // If we are accelerating
266 this->trapezoid_adjusted_rate
+= this->current_block
->rate_delta
;
267 if (this->trapezoid_adjusted_rate
> this->current_block
->nominal_rate
) {
268 this->trapezoid_adjusted_rate
= this->current_block
->nominal_rate
;
271 } else if (current_steps_completed
> this->current_block
->decelerate_after
) {
272 // If we are decelerating
274 // NOTE: We will only reduce speed if the result will be > 0. This catches small
275 // rounding errors that might leave steps hanging after the last trapezoid tick.
276 if(this->trapezoid_adjusted_rate
> this->current_block
->rate_delta
* 1.5F
) {
277 this->trapezoid_adjusted_rate
-= this->current_block
->rate_delta
;
279 this->trapezoid_adjusted_rate
= this->current_block
->rate_delta
* 1.5F
;
281 if(this->trapezoid_adjusted_rate
< this->current_block
->final_rate
) {
282 this->trapezoid_adjusted_rate
= this->current_block
->final_rate
;
285 } else if (trapezoid_adjusted_rate
!= current_block
->nominal_rate
) {
286 // If we are cruising
287 // Make sure we cruise at exactly nominal rate
288 this->trapezoid_adjusted_rate
= this->current_block
->nominal_rate
;
291 if(last_rate
!= trapezoid_adjusted_rate
) {
292 // don't call this if speed did not change
293 this->set_step_events_per_second(this->trapezoid_adjusted_rate
);
298 // Initializes the trapezoid generator from the current block. Called whenever a new
300 inline void Stepper::trapezoid_generator_reset()
302 this->trapezoid_adjusted_rate
= this->current_block
->initial_rate
;
303 this->force_speed_update
= true;
306 // Update the speed for all steppers
307 void Stepper::set_step_events_per_second( float steps_per_second
)
309 float isps
= steps_per_second
/ this->current_block
->steps_event_count
;
311 // Instruct the stepper motors
312 if( THEKERNEL
->robot
->alpha_stepper_motor
->moving
) {
313 THEKERNEL
->robot
->alpha_stepper_motor
->set_speed(isps
* this->current_block
->steps
[ALPHA_STEPPER
]);
315 if( THEKERNEL
->robot
->beta_stepper_motor
->moving
) {
316 THEKERNEL
->robot
->beta_stepper_motor
->set_speed(isps
* this->current_block
->steps
[BETA_STEPPER
]);
318 if( THEKERNEL
->robot
->gamma_stepper_motor
->moving
) {
319 THEKERNEL
->robot
->gamma_stepper_motor
->set_speed(isps
* this->current_block
->steps
[GAMMA_STEPPER
]);
322 // Other modules might want to know the speed changed
323 THEKERNEL
->call_event(ON_SPEED_CHANGE
, this);