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 #include "StepTicker.h"
  23
  24 #include <vector>
  25 using namespace std;
  26
  27 #include "libs/nuts_bolts.h"
  28 #include "libs/Hook.h"
  29
  30 #include <mri.h>
  31
  32 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
  33 // TODO: This does accel, accel should be in StepperMotor
  34
  35 Stepper::Stepper()
  36 {
  37     this->current_block = NULL;
  38     this->paused = false;
  39     this->force_speed_update = false;
  40     this->halted= false;
  41 }
  42
  43 //Called when the module has just been loaded
  44 void Stepper::on_module_loaded()
  45 {
  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     THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){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     // Steppers start off by default
  70     this->turn_enable_pins_off();
  71 }
  72
  73 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
  74 void Stepper::on_pause(void *argument)
  75 {
  76     this->paused = true;
  77     THEKERNEL->robot->alpha_stepper_motor->pause();
  78     THEKERNEL->robot->beta_stepper_motor->pause();
  79     THEKERNEL->robot->gamma_stepper_motor->pause();
  80 }
  81
  82 // When the play/pause button is set to play, or a module calls the ON_PLAY event
  83 void Stepper::on_play(void *argument)
  84 {
  85     // TODO: Re-compute the whole queue for a cold-start
  86     this->paused = false;
  87     THEKERNEL->robot->alpha_stepper_motor->unpause();
  88     THEKERNEL->robot->beta_stepper_motor->unpause();
  89     THEKERNEL->robot->gamma_stepper_motor->unpause();
  90 }
  91
  92 void Stepper::on_halt(void *argument)
  93 {
  94     if(argument == nullptr) {
  95         this->turn_enable_pins_off();
  96         this->halted= true;
  97     }else{
  98         this->halted= false;
  99     }
 100 }
 101
 102 void Stepper::on_gcode_received(void *argument)
 103 {
 104     Gcode *gcode = static_cast<Gcode *>(argument);
 105     // Attach gcodes to the last block for on_gcode_execute
 106     if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
 107         THEKERNEL->conveyor->append_gcode(gcode);
 108     }
 109 }
 110
 111 // React to enable/disable gcodes
 112 void Stepper::on_gcode_execute(void *argument)
 113 {
 114     Gcode *gcode = static_cast<Gcode *>(argument);
 115
 116     if( gcode->has_m) {
 117         if( gcode->m == 17 ) {
 118             this->turn_enable_pins_on();
 119         }
 120         if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) {
 121             this->turn_enable_pins_off();
 122         }
 123     }
 124 }
 125
 126 // Enable steppers
 127 void Stepper::turn_enable_pins_on()
 128 {
 129     for (StepperMotor *m : THEKERNEL->robot->actuators)
 130         m->enable(true);
 131     this->enable_pins_status = true;
 132 }
 133
 134 // Disable steppers
 135 void Stepper::turn_enable_pins_off()
 136 {
 137     for (StepperMotor *m : THEKERNEL->robot->actuators)
 138         m->enable(false);
 139     this->enable_pins_status = false;
 140 }
 141
 142 // A new block is popped from the queue
 143 void Stepper::on_block_begin(void *argument)
 144 {
 145     Block *block  = static_cast<Block *>(argument);
 146
 147     // The stepper does not care about 0-blocks
 148     if( block->millimeters == 0.0F ) {
 149         return;
 150     }
 151
 152     // Mark the new block as of interrest to us
 153     if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ) {
 154         block->take();
 155     } else {
 156         return;
 157     }
 158
 159     // We can't move with the enable pins off
 160     if( this->enable_pins_status == false ) {
 161         this->turn_enable_pins_on();
 162     }
 163
 164     // Setup : instruct stepper motors to move
 165     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
 166     this->main_stepper= nullptr;
 167     if( block->steps[ALPHA_STEPPER] > 0 ) {
 168         THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER])->set_moved_last_block(true);
 169         this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
 170     }else{
 171         THEKERNEL->robot->alpha_stepper_motor->set_moved_last_block(false);
 172     }
 173
 174     if( block->steps[BETA_STEPPER ] > 0 ) {
 175         THEKERNEL->robot->beta_stepper_motor->move(  block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ])->set_moved_last_block(true);
 176         if(this->main_stepper == nullptr || THEKERNEL->robot->beta_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
 177             this->main_stepper = THEKERNEL->robot->beta_stepper_motor;
 178     }else{
 179         THEKERNEL->robot->beta_stepper_motor->set_moved_last_block(false);
 180     }
 181     if( block->steps[GAMMA_STEPPER] > 0 ) {
 182         THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER])->set_moved_last_block(true);
 183         if(this->main_stepper == nullptr || THEKERNEL->robot->gamma_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
 184             this->main_stepper = THEKERNEL->robot->gamma_stepper_motor;
 185     }else{
 186         THEKERNEL->robot->gamma_stepper_motor->set_moved_last_block(false);
 187     }
 188
 189     this->current_block = block;
 190
 191     // Setup acceleration for this block
 192     this->trapezoid_generator_reset();
 193
 194     // Set the initial speed for this move
 195     this->trapezoid_generator_tick();
 196 }
 197
 198 // Current block is discarded
 199 void Stepper::on_block_end(void *argument)
 200 {
 201     this->current_block = NULL; //stfu !
 202 }
 203
 204 // When a stepper motor has finished it's assigned movement
 205 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy)
 206 {
 207     // We care only if none is still moving
 208     if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ) {
 209         return 0;
 210     }
 211
 212     // This block is finished, release it
 213     if( this->current_block != NULL ) {
 214         this->current_block->release();
 215     }
 216
 217     return 0;
 218 }
 219
 220
 221 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 222 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 223 // current_block stays untouched by outside handlers for the duration of this function call.
 224 void Stepper::trapezoid_generator_tick(void)
 225 {
 226     // Do not do the accel math for nothing
 227     if(this->current_block && !this->paused && this->main_stepper->moving ) {
 228
 229         // Store this here because we use it a lot down there
 230         uint32_t current_steps_completed = this->main_stepper->stepped;
 231         float last_rate= trapezoid_adjusted_rate;
 232
 233         if( this->force_speed_update ) {
 234             // Do not accel, just set the value
 235             this->force_speed_update = false;
 236             last_rate= -1;
 237
 238         } else if(THEKERNEL->conveyor->is_flushing()) {
 239             // if we are flushing the queue, decelerate to 0 then finish this block
 240             if (trapezoid_adjusted_rate > current_block->rate_delta * 1.5F) {
 241                 trapezoid_adjusted_rate -= current_block->rate_delta;
 242
 243             } else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) {
 244                 for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors
 245                 if (current_block) current_block->release();
 246                 THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped
 247                 return;
 248
 249             } else {
 250                 trapezoid_adjusted_rate = current_block->rate_delta * 0.5F;
 251             }
 252
 253         } else if(current_steps_completed <= this->current_block->accelerate_until) {
 254             // If we are accelerating
 255             // Increase speed
 256             this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 257             if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 258                 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 259             }
 260
 261         } else if (current_steps_completed > this->current_block->decelerate_after) {
 262             // If we are decelerating
 263             // Reduce speed
 264             // NOTE: We will only reduce speed if the result will be > 0. This catches small
 265             // rounding errors that might leave steps hanging after the last trapezoid tick.
 266             if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
 267                 this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 268             } else {
 269                 this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
 270             }
 271             if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 272                 this->trapezoid_adjusted_rate = this->current_block->final_rate;
 273             }
 274
 275         } else if (trapezoid_adjusted_rate != current_block->nominal_rate) {
 276             // If we are cruising
 277             // Make sure we cruise at exactly nominal rate
 278             this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 279         }
 280
 281         if(last_rate != trapezoid_adjusted_rate) {
 282             // don't call this if speed did not change
 283             this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 284         }
 285     }
 286 }
 287
 288 // Initializes the trapezoid generator from the current block. Called whenever a new
 289 // block begins.
 290 inline void Stepper::trapezoid_generator_reset()
 291 {
 292     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 293     this->force_speed_update = true;
 294 }
 295
 296 // Update the speed for all steppers
 297 void Stepper::set_step_events_per_second( float steps_per_second )
 298 {
 299     float isps= steps_per_second / this->current_block->steps_event_count;
 300
 301     // Instruct the stepper motors
 302     if( THEKERNEL->robot->alpha_stepper_motor->moving ) {
 303         THEKERNEL->robot->alpha_stepper_motor->set_speed(isps * this->current_block->steps[ALPHA_STEPPER]);
 304     }
 305     if( THEKERNEL->robot->beta_stepper_motor->moving  ) {
 306         THEKERNEL->robot->beta_stepper_motor->set_speed(isps * this->current_block->steps[BETA_STEPPER]);
 307     }
 308     if( THEKERNEL->robot->gamma_stepper_motor->moving ) {
 309         THEKERNEL->robot->gamma_stepper_motor->set_speed(isps * this->current_block->steps[GAMMA_STEPPER]);
 310     }
 311
 312     // Other modules might want to know the speed changed
 313     THEKERNEL->call_event(ON_SPEED_CHANGE, this);
 314 }
 315
 316