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