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
  20 #include <vector>
  21 using namespace std;
  22
  23 #include "libs/nuts_bolts.h"
  24 #include "libs/Hook.h"
  25
  26 #include <mri.h>
  27
  28
  29 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
  30 // TODO: This does accel, accel should be in StepperMotor
  31
  32 Stepper* stepper;
  33 uint32_t previous_step_count;
  34 uint32_t skipped_speed_updates;
  35 uint32_t speed_ticks_counter;
  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     skipped_speed_updates = 0;
  43 }
  44
  45 //Called when the module has just been loaded
  46 void Stepper::on_module_loaded(){
  47     stepper = this;
  48     register_for_event(ON_CONFIG_RELOAD);
  49     this->register_for_event(ON_BLOCK_BEGIN);
  50     this->register_for_event(ON_BLOCK_END);
  51     this->register_for_event(ON_GCODE_EXECUTE);
  52     this->register_for_event(ON_GCODE_RECEIVED);
  53     this->register_for_event(ON_PLAY);
  54     this->register_for_event(ON_PAUSE);
  55
  56     // Get onfiguration
  57     this->on_config_reload(this);
  58
  59     // Acceleration ticker
  60     this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
  61
  62     // Attach to the end_of_move stepper event
  63     THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
  64     THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
  65     THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
  66 }
  67
  68 // Get configuration from the config file
  69 void Stepper::on_config_reload(void* argument){
  70
  71     this->acceleration_ticks_per_second =  THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100   )->as_number();
  72     this->minimum_steps_per_second      =  THEKERNEL->config->value(minimum_steps_per_minute_checksum     )->by_default(3000  )->as_number() / 60.0F;
  73
  74     // Steppers start off by default
  75     this->turn_enable_pins_off();
  76 }
  77
  78 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
  79 void Stepper::on_pause(void* argument){
  80     this->paused = true;
  81     THEKERNEL->robot->alpha_stepper_motor->pause();
  82     THEKERNEL->robot->beta_stepper_motor->pause();
  83     THEKERNEL->robot->gamma_stepper_motor->pause();
  84 }
  85
  86 // When the play/pause button is set to play, or a module calls the ON_PLAY event
  87 void Stepper::on_play(void* argument){
  88     // TODO: Re-compute the whole queue for a cold-start
  89     this->paused = false;
  90     THEKERNEL->robot->alpha_stepper_motor->unpause();
  91     THEKERNEL->robot->beta_stepper_motor->unpause();
  92     THEKERNEL->robot->gamma_stepper_motor->unpause();
  93 }
  94
  95 void Stepper::on_gcode_received(void* argument){
  96     Gcode* gcode = static_cast<Gcode*>(argument);
  97     // Attach gcodes to the last block for on_gcode_execute
  98     if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
  99         THEKERNEL->conveyor->append_gcode(gcode);
 100     }
 101 }
 102
 103 // React to enable/disable gcodes
 104 void Stepper::on_gcode_execute(void* argument){
 105     Gcode* gcode = static_cast<Gcode*>(argument);
 106
 107     if( gcode->has_m){
 108         if( gcode->m == 17 ){
 109             this->turn_enable_pins_on();
 110         }
 111         if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ){
 112             this->turn_enable_pins_off();
 113         }
 114     }
 115 }
 116
 117 // Enable steppers
 118 void Stepper::turn_enable_pins_on(){
 119     for (StepperMotor* m : THEKERNEL->robot->actuators)
 120         m->enable(true);
 121     this->enable_pins_status = true;
 122 }
 123
 124 // Disable steppers
 125 void Stepper::turn_enable_pins_off(){
 126     for (StepperMotor* m : THEKERNEL->robot->actuators)
 127         m->enable(false);
 128     this->enable_pins_status = false;
 129 }
 130
 131 // A new block is popped from the queue
 132 void Stepper::on_block_begin(void* argument){
 133     Block* block  = static_cast<Block*>(argument);
 134
 135     // The stepper does not care about 0-blocks
 136     if( block->millimeters == 0.0F ){ return; }
 137
 138     // Mark the new block as of interrest to us
 139     if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ){
 140         block->take();
 141     }else{
 142         return;
 143     }
 144
 145     // We can't move with the enable pins off
 146     if( this->enable_pins_status == false ){
 147         this->turn_enable_pins_on();
 148     }
 149
 150     // Setup : instruct stepper motors to move
 151     if( block->steps[ALPHA_STEPPER] > 0 ){ THEKERNEL->robot->alpha_stepper_motor->move( ( block->direction_bits >> 0  ) & 1 , block->steps[ALPHA_STEPPER] ); }
 152     if( block->steps[BETA_STEPPER ] > 0 ){ THEKERNEL->robot->beta_stepper_motor->move(  ( block->direction_bits >> 1  ) & 1 , block->steps[BETA_STEPPER ] ); }
 153     if( block->steps[GAMMA_STEPPER] > 0 ){ THEKERNEL->robot->gamma_stepper_motor->move( ( block->direction_bits >> 2  ) & 1 , block->steps[GAMMA_STEPPER] ); }
 154
 155     this->current_block = block;
 156
 157     // Setup acceleration for this block
 158     this->trapezoid_generator_reset();
 159
 160     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
 161     this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
 162     if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->beta_stepper_motor; }
 163     if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; }
 164
 165     // Set the initial speed for this move
 166     this->trapezoid_generator_tick(0);
 167
 168     // Synchronise the acceleration curve with the stepping
 169     this->synchronize_acceleration(0);
 170
 171 }
 172
 173 // Current block is discarded
 174 void Stepper::on_block_end(void* argument){
 175     this->current_block = NULL; //stfu !
 176 }
 177
 178 // When a stepper motor has finished it's assigned movement
 179 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
 180
 181     // We care only if none is still moving
 182     if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ){ return 0; }
 183
 184     // This block is finished, release it
 185     if( this->current_block != NULL ){
 186         this->current_block->release();
 187     }
 188
 189     return 0;
 190 }
 191
 192
 193 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 194 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 195 // current_block stays untouched by outside handlers for the duration of this function call.
 196 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
 197
 198     // Do not do the accel math for nothing
 199     if(this->current_block && !this->paused && this->main_stepper->moving ) {
 200
 201         // Store this here because we use it a lot down there
 202         uint32_t current_steps_completed = this->main_stepper->stepped;
 203
 204         // Do not accel, just set the value
 205         if( this->force_speed_update ){
 206           this->force_speed_update = false;
 207           this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 208           return 0;
 209         }
 210
 211         // If we are accelerating
 212         if(current_steps_completed <= this->current_block->accelerate_until + 1) {
 213             // Increase speed
 214             this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 215               if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 216                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 217               }
 218               this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 219
 220         // If we are decelerating
 221         }else if (current_steps_completed > this->current_block->decelerate_after) {
 222              // Reduce speed
 223              // NOTE: We will only reduce speed if the result will be > 0. This catches small
 224               // rounding errors that might leave steps hanging after the last trapezoid tick.
 225               if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
 226                   this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 227               }else{
 228                   this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
 229               }
 230               if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 231                   this->trapezoid_adjusted_rate = this->current_block->final_rate;
 232               }
 233               this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 234
 235         // If we are cruising
 236         }else {
 237               // Make sure we cruise at exactly nominal rate
 238               if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
 239                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 240                   this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 241               }
 242           }
 243     }
 244
 245     return 0;
 246 }
 247
 248
 249
 250 // Initializes the trapezoid generator from the current block. Called whenever a new
 251 // block begins.
 252 inline void Stepper::trapezoid_generator_reset(){
 253     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 254     this->force_speed_update = true;
 255     this->trapezoid_tick_cycle_counter = 0;
 256     previous_step_count = 0;
 257     skipped_speed_updates = 0;
 258     speed_ticks_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
 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