src/modules/robot/Stepper.cpp

   1 /*
   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/>.
   6 */
   7
   8 #include "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "Stepper.h"
  11 #include "Planner.h"
  12 #include "Conveyor.h"
  13 #include <vector>
  14 using namespace std;
  15 #include "libs/nuts_bolts.h"
  16 #include "libs/Hook.h"
  17 #include <mri.h>
  18
  19
  20 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
  21 // TODO: This does accel, accel should be in StepperMotor
  22
  23 Stepper* stepper;
  24 uint32_t previous_step_count;
  25 uint32_t skipped_speed_updates;
  26 uint32_t speed_ticks_counter;
  27
  28 Stepper::Stepper(){
  29     this->current_block = NULL;
  30     this->paused = false;
  31     this->trapezoid_generator_busy = false;
  32     this->force_speed_update = false;
  33     skipped_speed_updates = 0;
  34 }
  35
  36 //Called when the module has just been loaded
  37 void Stepper::on_module_loaded(){
  38     stepper = this;
  39     register_for_event(ON_CONFIG_RELOAD);
  40     this->register_for_event(ON_BLOCK_BEGIN);
  41     this->register_for_event(ON_BLOCK_END);
  42     this->register_for_event(ON_GCODE_EXECUTE);
  43     this->register_for_event(ON_GCODE_RECEIVED);
  44     this->register_for_event(ON_PLAY);
  45     this->register_for_event(ON_PAUSE);
  46
  47     // Get onfiguration
  48     this->on_config_reload(this);
  49
  50     // Acceleration ticker
  51     this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
  52
  53     // Attach to the end_of_move stepper event
  54     THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
  55     THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
  56     THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
  57 }
  58
  59 // Get configuration from the config file
  60 void Stepper::on_config_reload(void* argument){
  61
  62     this->acceleration_ticks_per_second =  THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100   )->as_number();
  63     this->minimum_steps_per_minute      =  THEKERNEL->config->value(minimum_steps_per_minute_checksum     )->by_default(3000  )->as_number();
  64
  65     // Steppers start off by default
  66     this->turn_enable_pins_off();
  67 }
  68
  69 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
  70 void Stepper::on_pause(void* argument){
  71     this->paused = true;
  72     THEKERNEL->robot->alpha_stepper_motor->pause();
  73     THEKERNEL->robot->beta_stepper_motor->pause();
  74     THEKERNEL->robot->gamma_stepper_motor->pause();
  75 }
  76
  77 // When the play/pause button is set to play, or a module calls the ON_PLAY event
  78 void Stepper::on_play(void* argument){
  79     // TODO: Re-compute the whole queue for a cold-start
  80     this->paused = false;
  81     THEKERNEL->robot->alpha_stepper_motor->unpause();
  82     THEKERNEL->robot->beta_stepper_motor->unpause();
  83     THEKERNEL->robot->gamma_stepper_motor->unpause();
  84 }
  85
  86 void Stepper::on_gcode_received(void* argument){
  87     Gcode* gcode = static_cast<Gcode*>(argument);
  88     // Attach gcodes to the last block for on_gcode_execute
  89     if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
  90         gcode->mark_as_taken();
  91         if( THEKERNEL->conveyor->queue.size() == 0 ){
  92             THEKERNEL->call_event(ON_GCODE_EXECUTE, gcode );
  93         }else{
  94             Block* block = THEKERNEL->conveyor->queue.get_ref( THEKERNEL->conveyor->queue.size() - 1 );
  95             block->append_gcode(gcode);
  96         }
  97     }
  98 }
  99
 100 // React to enable/disable gcodes
 101 void Stepper::on_gcode_execute(void* argument){
 102     Gcode* gcode = static_cast<Gcode*>(argument);
 103
 104     if( gcode->has_m){
 105         if( gcode->m == 17 ){
 106             this->turn_enable_pins_on();
 107         }
 108         if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ){
 109             this->turn_enable_pins_off();
 110         }
 111     }
 112 }
 113
 114 // Enable steppers
 115 void Stepper::turn_enable_pins_on(){
 116     THEKERNEL->robot->alpha_en_pin.set(0);
 117     THEKERNEL->robot->beta_en_pin.set(0);
 118     THEKERNEL->robot->gamma_en_pin.set(0);
 119     this->enable_pins_status = true;
 120 }
 121
 122 // Disable steppers
 123 void Stepper::turn_enable_pins_off(){
 124     THEKERNEL->robot->alpha_en_pin.set(1);
 125     THEKERNEL->robot->beta_en_pin.set(1);
 126     THEKERNEL->robot->gamma_en_pin.set(1);
 127     this->enable_pins_status = false;
 128 }
 129
 130 // A new block is popped from the queue
 131 void Stepper::on_block_begin(void* argument){
 132     Block* block  = static_cast<Block*>(argument);
 133
 134     // The stepper does not care about 0-blocks
 135     if( block->millimeters == 0.0 ){ return; }
 136
 137     // Mark the new block as of interrest to us
 138     if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ){
 139         block->take();
 140     }else{
 141         return;
 142     }
 143
 144     // We can't move with the enable pins off
 145     if( this->enable_pins_status == false ){
 146         this->turn_enable_pins_on();
 147     }
 148
 149     // Setup : instruct stepper motors to move
 150     if( block->steps[ALPHA_STEPPER] > 0 ){ THEKERNEL->robot->alpha_stepper_motor->move( ( block->direction_bits >> 0  ) & 1 , block->steps[ALPHA_STEPPER] ); }
 151     if( block->steps[BETA_STEPPER ] > 0 ){ THEKERNEL->robot->beta_stepper_motor->move(  ( block->direction_bits >> 1  ) & 1 , block->steps[BETA_STEPPER ] ); }
 152     if( block->steps[GAMMA_STEPPER] > 0 ){ THEKERNEL->robot->gamma_stepper_motor->move( ( block->direction_bits >> 2  ) & 1 , block->steps[GAMMA_STEPPER] ); }
 153
 154     this->current_block = block;
 155
 156     // Setup acceleration for this block
 157     this->trapezoid_generator_reset();
 158
 159     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
 160     this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
 161     if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->beta_stepper_motor; }
 162     if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; }
 163
 164     // Set the initial speed for this move
 165     this->trapezoid_generator_tick(0);
 166
 167     // Synchronise the acceleration curve with the stepping
 168     this->synchronize_acceleration(0);
 169
 170 }
 171
 172 // Current block is discarded
 173 void Stepper::on_block_end(void* argument){
 174     this->current_block = NULL; //stfu !
 175 }
 176
 177 // When a stepper motor has finished it's assigned movement
 178 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
 179
 180     // We care only if none is still moving
 181     if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ){ return 0; }
 182
 183     // This block is finished, release it
 184     if( this->current_block != NULL ){
 185         this->current_block->release();
 186     }
 187
 188     return 0;
 189 }
 190
 191
 192 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 193 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 194 // current_block stays untouched by outside handlers for the duration of this function call.
 195 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
 196
 197     // Do not do the accel math for nothing
 198     if(this->current_block && !this->paused && this->main_stepper->moving ) {
 199
 200         // Store this here because we use it a lot down there
 201         uint32_t current_steps_completed = this->main_stepper->stepped;
 202
 203         // Do not accel, just set the value
 204         if( this->force_speed_update ){
 205           this->force_speed_update = false;
 206           this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 207           return 0;
 208         }
 209
 210         // If we are accelerating
 211         if(current_steps_completed <= this->current_block->accelerate_until + 1) {
 212             // Increase speed
 213             this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 214               if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 215                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 216               }
 217               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 218
 219         // If we are decelerating
 220         }else if (current_steps_completed > this->current_block->decelerate_after) {
 221              // Reduce speed
 222              // NOTE: We will only reduce speed if the result will be > 0. This catches small
 223               // rounding errors that might leave steps hanging after the last trapezoid tick.
 224               if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5) {
 225                   this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 226               }else{
 227                   this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5;
 228               }
 229               if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 230                   this->trapezoid_adjusted_rate = this->current_block->final_rate;
 231               }
 232               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 233
 234         // If we are cruising
 235         }else {
 236               // Make sure we cruise at exactly nominal rate
 237               if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
 238                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 239                   this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 240               }
 241           }
 242     }
 243
 244     return 0;
 245 }
 246
 247
 248
 249 // Initializes the trapezoid generator from the current block. Called whenever a new
 250 // block begins.
 251 inline void Stepper::trapezoid_generator_reset(){
 252     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 253     this->force_speed_update = true;
 254     this->trapezoid_tick_cycle_counter = 0;
 255     previous_step_count = 0;
 256     skipped_speed_updates = 0;
 257     speed_ticks_counter = 0;
 258 }
 259
 260 // Update the speed for all steppers
 261 void Stepper::set_step_events_per_minute( double steps_per_minute ){
 262
 263     // We do not step slower than this
 264     //steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
 265     if( steps_per_minute < this->minimum_steps_per_minute ){
 266         steps_per_minute = this->minimum_steps_per_minute;
 267     }
 268
 269     // Instruct the stepper motors
 270     if( THEKERNEL->robot->alpha_stepper_motor->moving ){ THEKERNEL->robot->alpha_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[ALPHA_STEPPER] / (double)this->current_block->steps_event_count ) ); }
 271     if( THEKERNEL->robot->beta_stepper_motor->moving  ){ THEKERNEL->robot->beta_stepper_motor->set_speed(  (steps_per_minute/60L) * ( (double)this->current_block->steps[BETA_STEPPER ] / (double)this->current_block->steps_event_count ) ); }
 272     if( THEKERNEL->robot->gamma_stepper_motor->moving ){ THEKERNEL->robot->gamma_stepper_motor->set_speed( (steps_per_minute/60L) * ( (double)this->current_block->steps[GAMMA_STEPPER] / (double)this->current_block->steps_event_count ) ); }
 273
 274     // Other modules might want to know the speed changed
 275     THEKERNEL->call_event(ON_SPEED_CHANGE, this);
 276
 277 }
 278
 279 // This function has the role of making sure acceleration and deceleration curves have their
 280 // rythm synchronized. The accel/decel must start at the same moment as the speed update routine
 281 // This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast.
 282 // All we do is reset the other timer so that it does what we want
 283 uint32_t Stepper::synchronize_acceleration(uint32_t dummy){
 284
 285     // No move was done, this is called from on_block_begin
 286     // This means we setup the accel timer in a way where it gets called right after
 287     // we exit this step interrupt, and so that it is then in synch with
 288     if( this->main_stepper->stepped == 0 ){
 289         // Whatever happens, we must call the accel interrupt asap
 290         // Because it will set the initial rate
 291         // We also want to synchronize in case we start accelerating or decelerating now
 292
 293         // Accel interrupt must happen asap
 294         NVIC_SetPendingIRQ(TIMER2_IRQn);
 295         // Synchronize both counters
 296         LPC_TIM2->TC = LPC_TIM0->TC;
 297
 298         // If we start decelerating after this, we must ask the actuator to warn us
 299         // so we can do what we do in the "else" bellow
 300         if( this->current_block->decelerate_after > 0 && this->current_block->decelerate_after < this->main_stepper->steps_to_move ){
 301             this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
 302         }
 303     }else{
 304         // If we are called not at the first steps, this means we are beginning deceleration
 305         NVIC_SetPendingIRQ(TIMER2_IRQn);
 306         // Synchronize both counters
 307         LPC_TIM2->TC = LPC_TIM0->TC;
 308     }
 309
 310     return 0;
 311 }
 312