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/>.
8 #include "libs/Module.h"
9 #include "libs/Kernel.h"
15 #include "libs/nuts_bolts.h"
16 #include "libs/Hook.h"
21 uint32_t previous_step_count
;
22 uint32_t skipped_speed_updates
;
23 uint32_t speed_ticks_counter
;
27 this->current_block
= NULL
;
29 this->trapezoid_generator_busy
= false;
30 this->force_speed_update
= false;
31 skipped_speed_updates
= 0;
34 //Called when the module has just been loaded
35 void Stepper::on_module_loaded(){
37 register_for_event(ON_CONFIG_RELOAD
);
38 this->register_for_event(ON_BLOCK_BEGIN
);
39 this->register_for_event(ON_BLOCK_END
);
40 this->register_for_event(ON_GCODE_EXECUTE
);
41 this->register_for_event(ON_PLAY
);
42 this->register_for_event(ON_PAUSE
);
45 this->on_config_reload(this);
47 // Acceleration ticker
48 this->acceleration_tick_hook
= this->kernel
->slow_ticker
->attach( this->acceleration_ticks_per_second
, this, &Stepper::trapezoid_generator_tick
);
50 // Attach to the end_of_move stepper event
51 this->kernel
->robot
->alpha_stepper_motor
->attach(this, &Stepper::stepper_motor_finished_move
);
52 this->kernel
->robot
->beta_stepper_motor
->attach( this, &Stepper::stepper_motor_finished_move
);
53 this->kernel
->robot
->gamma_stepper_motor
->attach(this, &Stepper::stepper_motor_finished_move
);
56 // Get configuration from the config file
57 void Stepper::on_config_reload(void* argument
){
59 this->acceleration_ticks_per_second
= this->kernel
->config
->value(acceleration_ticks_per_second_checksum
)->by_default(100 )->as_number();
60 this->minimum_steps_per_minute
= this->kernel
->config
->value(minimum_steps_per_minute_checksum
)->by_default(3000 )->as_number();
62 // Steppers start off by default
63 this->turn_enable_pins_off();
66 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
67 void Stepper::on_pause(void* argument
){
69 this->kernel
->robot
->alpha_stepper_motor
->pause();
70 this->kernel
->robot
->beta_stepper_motor
->pause();
71 this->kernel
->robot
->gamma_stepper_motor
->pause();
74 // When the play/pause button is set to play, or a module calls the ON_PLAY event
75 void Stepper::on_play(void* argument
){
76 // TODO: Re-compute the whole queue for a cold-start
78 this->kernel
->robot
->alpha_stepper_motor
->unpause();
79 this->kernel
->robot
->beta_stepper_motor
->unpause();
80 this->kernel
->robot
->gamma_stepper_motor
->unpause();
84 void Stepper::on_gcode_execute(void* argument
){
85 Gcode
* gcode
= static_cast<Gcode
*>(argument
);
89 this->turn_enable_pins_on();
91 if( gcode
->m
== 84 || gcode
->m
== 18 ){
92 this->turn_enable_pins_off();
97 void Stepper::turn_enable_pins_on(){
98 this->kernel
->robot
->alpha_en_pin
.set(0);
99 this->kernel
->robot
->beta_en_pin
.set(0);
100 this->kernel
->robot
->gamma_en_pin
.set(0);
101 this->enable_pins_status
= true;
104 void Stepper::turn_enable_pins_off(){
105 this->kernel
->robot
->alpha_en_pin
.set(1);
106 this->kernel
->robot
->beta_en_pin
.set(1);
107 this->kernel
->robot
->gamma_en_pin
.set(1);
108 this->enable_pins_status
= false;
111 // A new block is popped from the queue
112 void Stepper::on_block_begin(void* argument
){
113 Block
* block
= static_cast<Block
*>(argument
);
115 // The stepper does not care about 0-blocks
116 if( block
->millimeters
== 0.0 ){ return; }
118 // Mark the new block as of interrest to us
119 if( block
->steps
[ALPHA_STEPPER
] > 0 || block
->steps
[BETA_STEPPER
] > 0 || block
->steps
[GAMMA_STEPPER
] > 0 ){
125 // We can't move with the enable pins off
126 if( this->enable_pins_status
== false ){
127 this->turn_enable_pins_on();
130 // Setup : instruct stepper motors to move
131 if( block
->steps
[ALPHA_STEPPER
] > 0 ){ this->kernel
->robot
->alpha_stepper_motor
->move( ( block
->direction_bits
>> 0 ) & 1 , block
->steps
[ALPHA_STEPPER
] ); }
132 if( block
->steps
[BETA_STEPPER
] > 0 ){ this->kernel
->robot
->beta_stepper_motor
->move( ( block
->direction_bits
>> 1 ) & 1 , block
->steps
[BETA_STEPPER
] ); }
133 if( block
->steps
[GAMMA_STEPPER
] > 0 ){ this->kernel
->robot
->gamma_stepper_motor
->move( ( block
->direction_bits
>> 2 ) & 1 , block
->steps
[GAMMA_STEPPER
] ); }
135 this->current_block
= block
;
137 // Setup acceleration for this block
138 this->trapezoid_generator_reset();
140 // Find the stepper with the more steps, it's the one the speed calculations will want to follow
141 this->main_stepper
= this->kernel
->robot
->alpha_stepper_motor
;
142 if( this->kernel
->robot
->beta_stepper_motor
->steps_to_move
> this->main_stepper
->steps_to_move
){ this->main_stepper
= this->kernel
->robot
->beta_stepper_motor
; }
143 if( this->kernel
->robot
->gamma_stepper_motor
->steps_to_move
> this->main_stepper
->steps_to_move
){ this->main_stepper
= this->kernel
->robot
->gamma_stepper_motor
; }
145 // Synchronise the acceleration curve with the stepping
146 this->synchronize_acceleration(0);
150 // Current block is discarded
151 void Stepper::on_block_end(void* argument
){
152 this->current_block
= NULL
; //stfu !
155 //#pragma GCC push_options
156 //#pragma GCC optimize ("O0")
158 // When a stepper motor has finished it's assigned movement
159 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy
){
161 // We care only if none is still moving
162 if( this->kernel
->robot
->alpha_stepper_motor
->moving
|| this->kernel
->robot
->beta_stepper_motor
->moving
|| this->kernel
->robot
->gamma_stepper_motor
->moving
){ return 0; }
164 // This block is finished, release it
165 if( this->current_block
!= NULL
){
166 this->current_block
->release();
173 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
174 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
175 // current_block stays untouched by outside handlers for the duration of this function call.
176 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy
) {
178 LPC_GPIO1
->FIOSET
= 1<<31;
180 if(this->current_block
&& !this->paused
&& this->main_stepper
->moving
) {
181 uint32_t current_steps_completed
= this->main_stepper
->stepped
;
183 if( this->force_speed_update
){
184 this->force_speed_update
= false;
185 this->set_step_events_per_minute(this->trapezoid_adjusted_rate
);
186 LPC_GPIO1
->FIOCLR
= 1<<31;
190 if(current_steps_completed
<= this->current_block
->accelerate_until
+ 1) {
191 this->trapezoid_adjusted_rate
+= this->current_block
->rate_delta
;
193 if (this->trapezoid_adjusted_rate
> this->current_block
->nominal_rate
) {
194 this->trapezoid_adjusted_rate
= this->current_block
->nominal_rate
;
196 this->set_step_events_per_minute(this->trapezoid_adjusted_rate
);
197 }else if (current_steps_completed
> this->current_block
->decelerate_after
) {
198 // NOTE: We will only reduce speed if the result will be > 0. This catches small
199 // rounding errors that might leave steps hanging after the last trapezoid tick.
200 if(this->trapezoid_adjusted_rate
> this->current_block
->rate_delta
* 1.5) {
201 this->trapezoid_adjusted_rate
-= this->current_block
->rate_delta
;
203 this->trapezoid_adjusted_rate
= this->current_block
->rate_delta
* 1.5;
205 if(this->trapezoid_adjusted_rate
< this->current_block
->final_rate
) {
206 this->trapezoid_adjusted_rate
= this->current_block
->final_rate
;
208 this->set_step_events_per_minute(this->trapezoid_adjusted_rate
);
210 // Make sure we cruise at exactly nominal rate
211 if (this->trapezoid_adjusted_rate
!= this->current_block
->nominal_rate
) {
212 this->trapezoid_adjusted_rate
= this->current_block
->nominal_rate
;
213 this->set_step_events_per_minute(this->trapezoid_adjusted_rate
);
218 LPC_GPIO1
->FIOCLR
= 1<<31;
224 // Initializes the trapezoid generator from the current block. Called whenever a new
226 inline void Stepper::trapezoid_generator_reset(){
227 this->trapezoid_adjusted_rate
= this->current_block
->initial_rate
;
228 this->force_speed_update
= true;
229 this->trapezoid_tick_cycle_counter
= 0;
230 previous_step_count
= 0;
231 skipped_speed_updates
= 0;
232 speed_ticks_counter
= 0;
235 // Update the speed for all steppers
236 void Stepper::set_step_events_per_minute( double steps_per_minute
){
238 // We do not step slower than this
239 //steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
240 if( steps_per_minute
< this->minimum_steps_per_minute
){
241 steps_per_minute
= this->minimum_steps_per_minute
;
245 // Instruct the stepper motors
246 if( this->kernel
->robot
->alpha_stepper_motor
->moving
){ this->kernel
->robot
->alpha_stepper_motor
->set_speed( (steps_per_minute
/60L) * ( (double)this->current_block
->steps
[ALPHA_STEPPER
] / (double)this->current_block
->steps_event_count
) ); }
247 if( this->kernel
->robot
->beta_stepper_motor
->moving
){ this->kernel
->robot
->beta_stepper_motor
->set_speed( (steps_per_minute
/60L) * ( (double)this->current_block
->steps
[BETA_STEPPER
] / (double)this->current_block
->steps_event_count
) ); }
248 if( this->kernel
->robot
->gamma_stepper_motor
->moving
){ this->kernel
->robot
->gamma_stepper_motor
->set_speed( (steps_per_minute
/60L) * ( (double)this->current_block
->steps
[GAMMA_STEPPER
] / (double)this->current_block
->steps_event_count
) ); }
250 this->kernel
->call_event(ON_SPEED_CHANGE
, this);
254 // This function has the role of making sure acceleration and deceleration curves have their
255 // rythm synchronized. The accel/decel must start at the same moment as the speed update routine
256 // This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast.
257 // All we do is reset the other timer so that it does what we want
258 uint32_t Stepper::synchronize_acceleration(uint32_t dummy
){
260 LPC_GPIO1
->FIODIR
|= 1<<21;
261 LPC_GPIO1
->FIOSET
= 1<<21;
264 // No move was done, this is called from on_block_begin
265 // This means we setup the accel timer in a way where it gets called right after
266 // we exit this step interrupt, and so that it is then in synch with
267 if( this->main_stepper
->stepped
== 0 ){
268 // Whatever happens, we must call the accel interrupt asap
269 // Because it will set the initial rate
270 // We also want to synchronize in case we start accelerating or decelerating now
272 // Accel interrupt must happen asap
273 NVIC_SetPendingIRQ(TIMER2_IRQn
);
274 // Synchronize both counters
275 LPC_TIM2
->TC
= LPC_TIM0
->TC
;
277 // If we start decelerating after this, we must ask the actuator to warn us
278 // so we can do what we do in the "else" bellow
279 if( this->current_block
->decelerate_after
> 0 && this->current_block
->decelerate_after
< this->main_stepper
->steps_to_move
){
280 this->main_stepper
->attach_signal_step(this->current_block
->decelerate_after
, this, &Stepper::synchronize_acceleration
);
283 // If we are called not at the first steps, this means we are beginning deceleration
284 NVIC_SetPendingIRQ(TIMER2_IRQn
);
285 // Synchronize both counters
286 LPC_TIM2
->TC
= LPC_TIM0
->TC
;
289 LPC_GPIO1
->FIOCLR
= 1<<21;
295 //#pragma GCC pop_options