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->force_speed_update = false;
  39     this->halted= false;
  40 }
  41
  42 //Called when the module has just been loaded
  43 void Stepper::on_module_loaded()
  44 {
  45     this->register_for_event(ON_BLOCK_BEGIN);
  46     this->register_for_event(ON_BLOCK_END);
  47     this->register_for_event(ON_GCODE_EXECUTE);
  48     this->register_for_event(ON_GCODE_RECEIVED);
  49     this->register_for_event(ON_HALT);
  50
  51     // Get onfiguration
  52     this->on_config_reload(this);
  53
  54     // Acceleration ticker
  55     THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); });
  56
  57     // Attach to the end_of_move stepper event
  58     for (auto actuator : THEKERNEL->robot->actuators)
  59         actuator->attach(this, &Stepper::stepper_motor_finished_move );
  60 }
  61
  62 // Get configuration from the config file
  63 void Stepper::on_config_reload(void *argument)
  64 {
  65     // Steppers start off by default
  66     this->turn_enable_pins_off();
  67 }
  68
  69 void Stepper::on_halt(void *argument)
  70 {
  71     if(argument == nullptr) {
  72         this->turn_enable_pins_off();
  73         this->halted= true;
  74     }else{
  75         this->halted= false;
  76     }
  77 }
  78
  79 void Stepper::on_gcode_received(void *argument)
  80 {
  81     Gcode *gcode = static_cast<Gcode *>(argument);
  82     // Attach gcodes to the last block for on_gcode_execute
  83     if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
  84         THEKERNEL->conveyor->append_gcode(gcode);
  85     }
  86 }
  87
  88 // React to enable/disable gcodes
  89 void Stepper::on_gcode_execute(void *argument)
  90 {
  91     Gcode *gcode = static_cast<Gcode *>(argument);
  92
  93     if( gcode->has_m) {
  94         if( gcode->m == 17 ) {
  95             this->turn_enable_pins_on();
  96         }
  97         if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ) {
  98             this->turn_enable_pins_off();
  99         }
 100     }
 101 }
 102
 103 // Enable steppers
 104 void Stepper::turn_enable_pins_on()
 105 {
 106     for (auto a : THEKERNEL->robot->actuators)
 107         a->enable(true);
 108     this->enable_pins_status = true;
 109     THEKERNEL->call_event(ON_ENABLE, (void*)1);
 110 }
 111
 112 // Disable steppers
 113 void Stepper::turn_enable_pins_off()
 114 {
 115     for (auto a : THEKERNEL->robot->actuators)
 116         a->enable(false);
 117     this->enable_pins_status = false;
 118     THEKERNEL->call_event(ON_ENABLE, nullptr);
 119 }
 120
 121 // A new block is popped from the queue
 122 void Stepper::on_block_begin(void *argument)
 123 {
 124     Block *block  = static_cast<Block *>(argument);
 125
 126     // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
 127     bool take = false;
 128     if (block->millimeters > 0.0F) {
 129         for (size_t s = 0; !take && s < THEKERNEL->robot->actuators.size(); s++) {
 130             take = block->steps[s] > 0;
 131         }
 132     }
 133     if (take){
 134         block->take();
 135     } else {
 136         // none of the steppers move this block so make sure they know that
 137         for(auto a : THEKERNEL->robot->actuators) {
 138             a->set_moved_last_block(false);
 139         }
 140         return;
 141     }
 142
 143     // We can't move with the enable pins off
 144     if( this->enable_pins_status == false ) {
 145         this->turn_enable_pins_on();
 146     }
 147
 148     // Setup : instruct stepper motors to move
 149     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
 150     this->main_stepper = nullptr;
 151     int most_steps_to_move = 0;
 152     for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
 153         if (block->steps[i] > 0) {
 154             THEKERNEL->robot->actuators[i]->move(block->direction_bits[i], block->steps[i])->set_moved_last_block(true);
 155             int steps_to_move = THEKERNEL->robot->actuators[i]->get_steps_to_move();
 156             if (steps_to_move > most_steps_to_move) {
 157                 most_steps_to_move = steps_to_move;
 158                 this->main_stepper = THEKERNEL->robot->actuators[i];
 159             }
 160         }
 161         else {
 162             THEKERNEL->robot->actuators[i]->set_moved_last_block(false);
 163         }
 164     }
 165
 166     this->current_block = block;
 167
 168     // Setup acceleration for this block
 169     this->trapezoid_generator_reset();
 170
 171     // Set the initial speed for this move
 172     this->trapezoid_generator_tick();
 173
 174     // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block
 175     THEKERNEL->step_ticker->synchronize_acceleration(false);
 176
 177     // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step
 178     if( block->decelerate_after > 0 && block->decelerate_after+1 < this->main_stepper->steps_to_move ) {
 179         this->main_stepper->signal_step= block->decelerate_after+1; // we make it +1 as deceleration does not start until steps > decelerate_after
 180     }
 181 }
 182
 183 // Current block is discarded
 184 void Stepper::on_block_end(void *argument)
 185 {
 186     this->current_block = NULL; //stfu !
 187 }
 188
 189 // When a stepper motor has finished it's assigned movement
 190 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy)
 191 {
 192     // We care only if none is still moving
 193     for (auto a : THEKERNEL->robot->actuators) {
 194         if(a->moving)
 195             return 0;
 196     }
 197
 198     // This block is finished, release it
 199     if( this->current_block != NULL ) {
 200         this->current_block->release();
 201     }
 202
 203     return 0;
 204 }
 205
 206
 207 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 208 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 209 // current_block stays untouched by outside handlers for the duration of this function call.
 210 // NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv
 211 void Stepper::trapezoid_generator_tick(void)
 212 {
 213     // Do not do the accel math for nothing
 214     if(this->current_block && this->main_stepper->moving ) {
 215
 216         // Store this here because we use it a lot down there
 217         uint32_t current_steps_completed = this->main_stepper->stepped;
 218         float last_rate= trapezoid_adjusted_rate;
 219
 220         if( this->force_speed_update ) {
 221             // Do not accel, just set the value
 222             this->force_speed_update = false;
 223             last_rate= -1;
 224
 225         } else if(THEKERNEL->conveyor->is_flushing()) {
 226             // if we are flushing the queue, decelerate to 0 then finish this block
 227             if (trapezoid_adjusted_rate > current_block->rate_delta * 1.5F) {
 228                 trapezoid_adjusted_rate -= current_block->rate_delta;
 229
 230             } else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) {
 231                 for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors
 232                 if (current_block) current_block->release();
 233                 THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped
 234                 return;
 235
 236             } else {
 237                 trapezoid_adjusted_rate = current_block->rate_delta * 0.5F;
 238             }
 239
 240         } else if(current_steps_completed <= this->current_block->accelerate_until) {
 241             // If we are accelerating
 242             // Increase speed
 243             this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 244             if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 245                 this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 246             }
 247
 248         } else if (current_steps_completed > this->current_block->decelerate_after) {
 249             // If we are decelerating
 250             // Reduce speed
 251             // NOTE: We will only reduce speed if the result will be > 0. This catches small
 252             // rounding errors that might leave steps hanging after the last trapezoid tick.
 253             if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
 254                 this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 255             } else {
 256                 this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
 257             }
 258             if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 259                 this->trapezoid_adjusted_rate = this->current_block->final_rate;
 260             }
 261
 262         } else if (trapezoid_adjusted_rate != current_block->nominal_rate) {
 263             // If we are cruising
 264             // Make sure we cruise at exactly nominal rate
 265             this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 266         }
 267
 268         if(last_rate != trapezoid_adjusted_rate) {
 269             // don't call this if speed did not change
 270             this->set_step_events_per_second(this->trapezoid_adjusted_rate);
 271         }
 272     }
 273 }
 274
 275 // Initializes the trapezoid generator from the current block. Called whenever a new
 276 // block begins.
 277 inline void Stepper::trapezoid_generator_reset()
 278 {
 279     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 280     this->force_speed_update = true;
 281 }
 282
 283 // Update the speed for all steppers
 284 void Stepper::set_step_events_per_second( float steps_per_second )
 285 {
 286     float isps= steps_per_second / this->current_block->steps_event_count;
 287
 288     // Instruct the stepper motors
 289     for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
 290         if (THEKERNEL->robot->actuators[i]->moving) {
 291             THEKERNEL->robot->actuators[i]->set_speed(isps * this->current_block->steps[i]);
 292         }
 293     }
 294
 295     // Other modules might want to know the speed changed
 296     THEKERNEL->call_event(ON_SPEED_CHANGE, this);
 297 }
 298
 299