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 "libs/Module.h"
00009 #include "libs/Kernel.h"
00010 #include "Stepper.h"
00011 #include "Planner.h"
00012 #include "Player.h"
00013 #include <vector>
00014 using namespace std;
00015 #include "libs/nuts_bolts.h"
00016 
00017 Stepper* stepper;
00018 
00019 Stepper::Stepper(){
00020     this->current_block = NULL;
00021     this->step_events_completed = 0; 
00022     this->divider = 0;
00023     this->paused = false;
00024 }
00025 
00026 //Called when the module has just been loaded
00027 void Stepper::on_module_loaded(){
00028     stepper = this;
00029     this->register_for_event(ON_BLOCK_BEGIN);
00030     this->register_for_event(ON_BLOCK_END);
00031     this->register_for_event(ON_GCODE_EXECUTE);
00032     this->register_for_event(ON_PLAY);
00033     this->register_for_event(ON_PAUSE);
00034  
00035     // Get onfiguration
00036     this->on_config_reload(this); 
00037 
00038     // Acceleration ticker
00039     //this->kernel->slow_ticker->set_frequency(this->acceleration_ticks_per_second/10);
00040     this->kernel->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
00041 
00042     // Initiate main_interrupt timer and step reset timer
00043     this->kernel->step_ticker->attach( this, &Stepper::main_interrupt );   
00044     this->kernel->step_ticker->reset_attach( this, &Stepper::reset_step_pins );
00045 
00046 }
00047 
00048 // Get configuration from the config file
00049 void Stepper::on_config_reload(void* argument){
00050     
00051     this->microseconds_per_step_pulse   =  this->kernel->config->value(microseconds_per_step_pulse_checksum  )->by_default(5     )->as_number();
00052     this->acceleration_ticks_per_second =  this->kernel->config->value(acceleration_ticks_per_second_checksum)->by_default(100   )->as_number();
00053     this->minimum_steps_per_minute      =  this->kernel->config->value(minimum_steps_per_minute_checksum     )->by_default(1200  )->as_number();
00054     this->base_stepping_frequency       =  this->kernel->config->value(base_stepping_frequency_checksum      )->by_default(100000)->as_number();
00055     this->alpha_step_pin                =  this->kernel->config->value(alpha_step_pin_checksum               )->by_default("1.21"     )->as_pin()->as_output();
00056     this->beta_step_pin                 =  this->kernel->config->value(beta_step_pin_checksum                )->by_default("1.23"     )->as_pin()->as_output();
00057     this->gamma_step_pin                =  this->kernel->config->value(gamma_step_pin_checksum               )->by_default("1.22!"    )->as_pin()->as_output();
00058     this->alpha_dir_pin                 =  this->kernel->config->value(alpha_dir_pin_checksum                )->by_default("1.18"     )->as_pin()->as_output();
00059     this->beta_dir_pin                  =  this->kernel->config->value(beta_dir_pin_checksum                 )->by_default("1.20"     )->as_pin()->as_output();
00060     this->gamma_dir_pin                 =  this->kernel->config->value(gamma_dir_pin_checksum                )->by_default("1.19"     )->as_pin()->as_output();
00061     this->alpha_en_pin                  =  this->kernel->config->value(alpha_en_pin_checksum                 )->by_default("0.4"      )->as_pin()->as_output()->as_open_drain();
00062     this->beta_en_pin                   =  this->kernel->config->value(beta_en_pin_checksum                  )->by_default("0.10"     )->as_pin()->as_output()->as_open_drain();
00063     this->gamma_en_pin                  =  this->kernel->config->value(gamma_en_pin_checksum                 )->by_default("0.19"     )->as_pin()->as_output()->as_open_drain();
00064 
00065 
00066     // TODO : This is supposed to be done by gcodes
00067     this->alpha_en_pin->set(0);
00068     this->beta_en_pin->set(0);
00069     this->gamma_en_pin->set(0);
00070 
00071 
00072     // Set the Timer interval for Match Register 1, 
00073     this->kernel->step_ticker->set_reset_delay( this->microseconds_per_step_pulse / 1000000 );
00074     this->kernel->step_ticker->set_frequency( this->base_stepping_frequency );
00075 }
00076 
00077 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
00078 void Stepper::on_pause(void* argument){
00079     this->paused = true;
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 }
00087 
00088 void Stepper::on_gcode_execute(void* argument){
00089     Gcode* gcode = static_cast<Gcode*>(argument);
00090 
00091     if( gcode->has_letter('M')){
00092         int code = (int) gcode->get_value('M');
00093         if( code == 84 ){
00094             this->alpha_en_pin->set(0);
00095             this->beta_en_pin->set(0);
00096             this->gamma_en_pin->set(0);
00097         }
00098     }
00099 }
00100 
00101 // A new block is popped from the queue
00102 void Stepper::on_block_begin(void* argument){
00103     Block* block  = static_cast<Block*>(argument);
00104 
00105     // The stepper does not care about 0-blocks
00106     if( block->millimeters == 0.0 ){ return; }
00107     
00108     // Mark the new block as of interrest to us
00109     block->take();
00110    
00111     // Setup
00112     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; } 
00113     this->step_events_completed = 0; 
00114 
00115     this->current_block = block;
00116     
00117     // This is just to save computing power and not do it every step 
00118     this->update_offsets();
00119     
00120     // Setup acceleration for this block 
00121     this->trapezoid_generator_reset();
00122 
00123 }
00124 
00125 // Current block is discarded
00126 void Stepper::on_block_end(void* argument){
00127     Block* block  = static_cast<Block*>(argument);
00128     this->current_block = NULL; //stfu !
00129 }
00130 
00131 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Smoothie. It is executed at the rate set with
00132 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
00133 inline uint32_t Stepper::main_interrupt(uint32_t dummy){
00134     if( this->paused ){ return 0; } 
00135 
00136     // Step dir pins first, then step pinse, stepper drivers like to know the direction before the step signal comes in
00137     this->alpha_dir_pin->set(  ( this->out_bits >> 0  ) & 1 );
00138     this->beta_dir_pin->set(   ( this->out_bits >> 1  ) & 1 );
00139     this->gamma_dir_pin->set(  ( this->out_bits >> 2  ) & 1 );
00140     this->alpha_step_pin->set( ( this->out_bits >> 3  ) & 1 );
00141     this->beta_step_pin->set(  ( this->out_bits >> 4  ) & 1 );
00142     this->gamma_step_pin->set( ( this->out_bits >> 5  ) & 1 );
00143 
00144     if( this->current_block != NULL ){
00145         // Set bits for direction and steps 
00146         this->out_bits = this->current_block->direction_bits;
00147         for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ 
00148             this->counters[stpr] += this->counter_increment; 
00149             if( this->counters[stpr] > this->offsets[stpr] && this->stepped[stpr] < this->current_block->steps[stpr] ){
00150                 this->counters[stpr] -= this->offsets[stpr] ;
00151                 this->stepped[stpr]++;
00152                 this->out_bits |= (1 << (stpr+3) );
00153             } 
00154         } 
00155         // If current block is finished, reset pointer
00156         this->step_events_completed += this->counter_increment;
00157         if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){ 
00158             if( this->stepped[ALPHA_STEPPER] == this->current_block->steps[ALPHA_STEPPER] && this->stepped[BETA_STEPPER] == this->current_block->steps[BETA_STEPPER] && this->stepped[GAMMA_STEPPER] == this->current_block->steps[GAMMA_STEPPER] ){ 
00159                 if( this->current_block != NULL ){
00160                     this->current_block->release(); 
00161                 }
00162             }
00163         }
00164     }else{
00165         this->out_bits = 0;
00166     }
00167 
00168 }
00169 
00170 // We compute this here instead of each time in the interrupt
00171 void Stepper::update_offsets(){
00172     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ 
00173         this->offsets[stpr] = (int)floor((float)((float)(1<<16)*(float)((float)this->current_block->steps_event_count / (float)this->current_block->steps[stpr]))); 
00174     }
00175 }
00176 
00177 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
00178 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
00179 // current_block stays untouched by outside handlers for the duration of this function call.
00180 uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy ) {
00181     if(this->current_block && !this->trapezoid_generator_busy && !this->paused ) {
00182           if(this->step_events_completed < this->current_block->accelerate_until<<16) {
00183               this->trapezoid_adjusted_rate += this->current_block->rate_delta;
00184               if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
00185                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
00186               }
00187               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
00188           } else if (this->step_events_completed >= this->current_block->decelerate_after<<16) {
00189               // NOTE: We will only reduce speed if the result will be > 0. This catches small
00190               // rounding errors that might leave steps hanging after the last trapezoid tick.
00191               if (this->trapezoid_adjusted_rate > double(this->current_block->rate_delta) * 1.5) {
00192                   this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
00193               }else{
00194                   this->trapezoid_adjusted_rate = double(this->current_block->rate_delta) * 1.5; 
00195                   //this->trapezoid_adjusted_rate = floor(double(this->trapezoid_adjusted_rate / 2 ));
00196                   //this->kernel->serial->printf("over!\r\n");
00197               }
00198               if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
00199                   this->trapezoid_adjusted_rate = this->current_block->final_rate;
00200               }
00201               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
00202           } else {
00203               // Make sure we cruise at exactly nominal rate
00204               if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
00205                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
00206                   this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
00207               }
00208           }
00209     }
00210 }
00211 
00212 // Initializes the trapezoid generator from the current block. Called whenever a new
00213 // block begins.
00214 void Stepper::trapezoid_generator_reset(){
00215     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
00216     this->trapezoid_tick_cycle_counter = 0;
00217     // Because this can be called directly from the main loop, it could be interrupted by the acceleration ticker, and that would be bad, so we use a flag
00218     this->trapezoid_generator_busy = true;
00219     this->set_step_events_per_minute(this->trapezoid_adjusted_rate); 
00220     this->trapezoid_generator_busy = false;
00221 }
00222 
00223 void Stepper::set_step_events_per_minute( double steps_per_minute ){
00224 
00225     // We do not step slower than this 
00226     steps_per_minute = max(steps_per_minute, this->minimum_steps_per_minute);
00227 
00228     // The speed factor is the factor by which we must multiply the minimal step frequency to reach the maximum step frequency
00229     // The higher, the smoother the movement will be, but the closer the maximum and minimum frequencies are, the smaller the factor is
00230     // speed_factor = base_stepping_frequency / steps_per_second
00231     // counter_increment = 1 / speed_factor ( 1 is 1<<16 because we do fixed point )
00232     this->counter_increment = int(floor(double(1<<16)/double(this->base_stepping_frequency / (steps_per_minute/60L))));
00233 
00234     this->kernel->call_event(ON_SPEED_CHANGE, this);
00235 
00236 }
00237 
00238 uint32_t Stepper::reset_step_pins(uint32_t dummy){
00239     this->alpha_step_pin->set(0);
00240     this->beta_step_pin->set(0); 
00241     this->gamma_step_pin->set(0);
00242 
00243 }