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