Stepper.cpp

00001 /*
00002       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)
00003       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.
00004       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.
00005       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
00006 */
00007 
00008 #include "Stepper.h"
00009 
00010 #include "libs/Module.h"
00011 #include "libs/Kernel.h"
00012 #include "Planner.h"
00013 #include "Conveyor.h"
00014 #include "StepperMotor.h"
00015 
00016 #include <vector>
00017 using namespace std;
00018 
00019 #include "libs/nuts_bolts.h"
00020 #include "libs/Hook.h"
00021 
00022 #include <mri.h>
00023 
00024 
00025 // The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
00026 // TODO: This does accel, accel should be in StepperMotor
00027 
00028 Stepper* stepper;
00029 uint32_t previous_step_count;
00030 uint32_t skipped_speed_updates;
00031 uint32_t speed_ticks_counter;
00032 
00033 Stepper::Stepper(){
00034     this->current_block = NULL;
00035     this->paused = false;
00036     this->trapezoid_generator_busy = false;
00037     this->force_speed_update = false;
00038     skipped_speed_updates = 0;
00039 }
00040 
00041 //Called when the module has just been loaded
00042 void Stepper::on_module_loaded(){
00043     stepper = this;
00044     register_for_event(ON_CONFIG_RELOAD);
00045     this->register_for_event(ON_BLOCK_BEGIN);
00046     this->register_for_event(ON_BLOCK_END);
00047     this->register_for_event(ON_GCODE_EXECUTE);
00048     this->register_for_event(ON_GCODE_RECEIVED);
00049     this->register_for_event(ON_PLAY);
00050     this->register_for_event(ON_PAUSE);
00051 
00052     // Get onfiguration
00053     this->on_config_reload(this);
00054 
00055     // Acceleration ticker
00056     this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
00057 
00058     // Attach to the end_of_move stepper event
00059     THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
00060     THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
00061     THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
00062 }
00063 
00064 // Get configuration from the config file
00065 void Stepper::on_config_reload(void* argument){
00066 
00067     this->acceleration_ticks_per_second =  THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100   )->as_number();
00068     this->minimum_steps_per_second      =  THEKERNEL->config->value(minimum_steps_per_minute_checksum     )->by_default(3000  )->as_number() / 60.0F;
00069 
00070     // Steppers start off by default
00071     this->turn_enable_pins_off();
00072 }
00073 
00074 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
00075 void Stepper::on_pause(void* argument){
00076     this->paused = true;
00077     THEKERNEL->robot->alpha_stepper_motor->pause();
00078     THEKERNEL->robot->beta_stepper_motor->pause();
00079     THEKERNEL->robot->gamma_stepper_motor->pause();
00080 }
00081 
00082 // When the play/pause button is set to play, or a module calls the ON_PLAY event
00083 void Stepper::on_play(void* argument){
00084     // TODO: Re-compute the whole queue for a cold-start
00085     this->paused = false;
00086     THEKERNEL->robot->alpha_stepper_motor->unpause();
00087     THEKERNEL->robot->beta_stepper_motor->unpause();
00088     THEKERNEL->robot->gamma_stepper_motor->unpause();
00089 }
00090 
00091 void Stepper::on_gcode_received(void* argument){
00092     Gcode* gcode = static_cast<Gcode*>(argument);
00093     // Attach gcodes to the last block for on_gcode_execute
00094     if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
00095         THEKERNEL->conveyor->append_gcode(gcode);
00096     }
00097 }
00098 
00099 // React to enable/disable gcodes
00100 void Stepper::on_gcode_execute(void* argument){
00101     Gcode* gcode = static_cast<Gcode*>(argument);
00102 
00103     if( gcode->has_m){
00104         if( gcode->m == 17 ){
00105             this->turn_enable_pins_on();
00106         }
00107         if( (gcode->m == 84 || gcode->m == 18) && !gcode->has_letter('E') ){
00108             this->turn_enable_pins_off();
00109         }
00110     }
00111 }
00112 
00113 // Enable steppers
00114 void Stepper::turn_enable_pins_on(){
00115     for (StepperMotor* m : THEKERNEL->robot->actuators)
00116         m->enable(true);
00117     this->enable_pins_status = true;
00118 }
00119 
00120 // Disable steppers
00121 void Stepper::turn_enable_pins_off(){
00122     for (StepperMotor* m : THEKERNEL->robot->actuators)
00123         m->enable(false);
00124     this->enable_pins_status = false;
00125 }
00126 
00127 // A new block is popped from the queue
00128 void Stepper::on_block_begin(void* argument){
00129     Block* block  = static_cast<Block*>(argument);
00130 
00131     // The stepper does not care about 0-blocks
00132     if( block->millimeters == 0.0F ){ return; }
00133 
00134     // Mark the new block as of interrest to us
00135     if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ){
00136         block->take();
00137     }else{
00138         return;
00139     }
00140 
00141     // We can't move with the enable pins off
00142     if( this->enable_pins_status == false ){
00143         this->turn_enable_pins_on();
00144     }
00145 
00146     // Setup : instruct stepper motors to move
00147     if( block->steps[ALPHA_STEPPER] > 0 ){ THEKERNEL->robot->alpha_stepper_motor->move( ( block->direction_bits >> 0  ) & 1 , block->steps[ALPHA_STEPPER] ); }
00148     if( block->steps[BETA_STEPPER ] > 0 ){ THEKERNEL->robot->beta_stepper_motor->move(  ( block->direction_bits >> 1  ) & 1 , block->steps[BETA_STEPPER ] ); }
00149     if( block->steps[GAMMA_STEPPER] > 0 ){ THEKERNEL->robot->gamma_stepper_motor->move( ( block->direction_bits >> 2  ) & 1 , block->steps[GAMMA_STEPPER] ); }
00150 
00151     this->current_block = block;
00152 
00153     // Setup acceleration for this block
00154     this->trapezoid_generator_reset();
00155 
00156     // Find the stepper with the more steps, it's the one the speed calculations will want to follow
00157     this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
00158     if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->beta_stepper_motor; }
00159     if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ){ this->main_stepper = THEKERNEL->robot->gamma_stepper_motor; }
00160 
00161     // Set the initial speed for this move
00162     this->trapezoid_generator_tick(0);
00163 
00164     // Synchronise the acceleration curve with the stepping
00165     this->synchronize_acceleration(0);
00166 
00167 }
00168 
00169 // Current block is discarded
00170 void Stepper::on_block_end(void* argument){
00171     this->current_block = NULL; //stfu !
00172 }
00173 
00174 // When a stepper motor has finished it's assigned movement
00175 uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
00176 
00177     // We care only if none is still moving
00178     if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ){ return 0; }
00179 
00180     // This block is finished, release it
00181     if( this->current_block != NULL ){
00182         this->current_block->release();
00183     }
00184 
00185     return 0;
00186 }
00187 
00188 
00189 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
00190 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
00191 // current_block stays untouched by outside handlers for the duration of this function call.
00192 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
00193 
00194     // Do not do the accel math for nothing
00195     if(this->current_block && !this->paused && this->main_stepper->moving ) {
00196 
00197         // Store this here because we use it a lot down there
00198         uint32_t current_steps_completed = this->main_stepper->stepped;
00199 
00200         // Do not accel, just set the value
00201         if( this->force_speed_update ){
00202           this->force_speed_update = false;
00203           this->set_step_events_per_second(this->trapezoid_adjusted_rate);
00204           return 0;
00205         }
00206 
00207         // If we are accelerating
00208         if(current_steps_completed <= this->current_block->accelerate_until + 1) {
00209             // Increase speed
00210             this->trapezoid_adjusted_rate += this->current_block->rate_delta;
00211               if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
00212                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
00213               }
00214               this->set_step_events_per_second(this->trapezoid_adjusted_rate);
00215 
00216         // If we are decelerating
00217         }else if (current_steps_completed > this->current_block->decelerate_after) {
00218              // Reduce speed
00219              // NOTE: We will only reduce speed if the result will be > 0. This catches small
00220               // rounding errors that might leave steps hanging after the last trapezoid tick.
00221               if(this->trapezoid_adjusted_rate > this->current_block->rate_delta * 1.5F) {
00222                   this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
00223               }else{
00224                   this->trapezoid_adjusted_rate = this->current_block->rate_delta * 1.5F;
00225               }
00226               if(this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
00227                   this->trapezoid_adjusted_rate = this->current_block->final_rate;
00228               }
00229               this->set_step_events_per_second(this->trapezoid_adjusted_rate);
00230 
00231         // If we are cruising
00232         }else {
00233               // Make sure we cruise at exactly nominal rate
00234               if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
00235                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
00236                   this->set_step_events_per_second(this->trapezoid_adjusted_rate);
00237               }
00238           }
00239     }
00240 
00241     return 0;
00242 }
00243 
00244 
00245 
00246 // Initializes the trapezoid generator from the current block. Called whenever a new
00247 // block begins.
00248 inline void Stepper::trapezoid_generator_reset(){
00249     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
00250     this->force_speed_update = true;
00251     this->trapezoid_tick_cycle_counter = 0;
00252     previous_step_count = 0;
00253     skipped_speed_updates = 0;
00254     speed_ticks_counter = 0;
00255 }
00256 
00257 // Update the speed for all steppers
00258 void Stepper::set_step_events_per_second( float steps_per_second )
00259 {
00260     // We do not step slower than this
00261     //steps_per_second = max(steps_per_second, this->minimum_steps_per_second);
00262     if( steps_per_second < this->minimum_steps_per_second ){
00263         steps_per_second = this->minimum_steps_per_second;
00264     }
00265 
00266     // Instruct the stepper motors
00267     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 ) ); }
00268     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 ) ); }
00269     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 ) ); }
00270 
00271     // Other modules might want to know the speed changed
00272     THEKERNEL->call_event(ON_SPEED_CHANGE, this);
00273 
00274 }
00275 
00276 // This function has the role of making sure acceleration and deceleration curves have their
00277 // rhythm synchronized. The accel/decel must start at the same moment as the speed update routine
00278 // This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast.
00279 // All we do is reset the other timer so that it does what we want
00280 uint32_t Stepper::synchronize_acceleration(uint32_t dummy){
00281 
00282     // No move was done, this is called from on_block_begin
00283     // This means we setup the accel timer in a way where it gets called right after
00284     // we exit this step interrupt, and so that it is then in synch with
00285     if( this->main_stepper->stepped == 0 ){
00286         // Whatever happens, we must call the accel interrupt asap
00287         // Because it will set the initial rate
00288         // We also want to synchronize in case we start accelerating or decelerating now
00289 
00290         // Accel interrupt must happen asap
00291         NVIC_SetPendingIRQ(TIMER2_IRQn);
00292         // Synchronize both counters
00293         LPC_TIM2->TC = LPC_TIM0->TC;
00294 
00295         // If we start decelerating after this, we must ask the actuator to warn us
00296         // so we can do what we do in the "else" bellow
00297         if( this->current_block->decelerate_after > 0 && this->current_block->decelerate_after < this->main_stepper->steps_to_move ){
00298             this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
00299         }
00300     }else{
00301         // If we are called not at the first steps, this means we are beginning deceleration
00302         NVIC_SetPendingIRQ(TIMER2_IRQn);
00303         // Synchronize both counters
00304         LPC_TIM2->TC = LPC_TIM0->TC;
00305     }
00306 
00307     return 0;
00308 }
00309