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