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).
   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 "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "Stepper.h"
  11 #include "Planner.h"
  12 #include "mbed.h"
  13 #include <vector>
  14 using namespace std;
  15 #include "libs/nuts_bolts.h"
  16
  17 Timeout flipper;
  18 Stepper* stepper;
  19
  20 Stepper::Stepper(){
  21     this->current_block = NULL;
  22     this->step_events_completed = 0;
  23     this->divider = 0;
  24 }
  25
  26 //Called when the module has just been loaded
  27 void Stepper::on_module_loaded(){
  28     stepper = this;
  29     this->register_for_event(ON_STEPPER_WAKE_UP);
  30     this->register_for_event(ON_PLAY);
  31     this->register_for_event(ON_PAUSE);
  32
  33     // Get onfiguration
  34     this->on_config_reload(this);
  35
  36     // Acceleration timer
  37     this->acceleration_ticker.attach_us(this, &Stepper::trapezoid_generator_tick, 1000000/this->acceleration_ticks_per_second);
  38
  39     // Initiate main_interrupt timer and step reset timer
  40     LPC_TIM0->MR0 = 10000;
  41     LPC_TIM0->MCR = 11; // for MR0 and MR1, with no reset at MR1
  42     NVIC_EnableIRQ(TIMER0_IRQn);
  43     NVIC_SetPriority(TIMER3_IRQn, 1);
  44     LPC_TIM0->TCR = 1;
  45
  46     // Step and Dir pins as outputs
  47     this->step_gpio_port->FIODIR |= this->step_mask;
  48     this->dir_gpio_port->FIODIR  |= this->dir_mask;
  49
  50 }
  51
  52 void Stepper::on_config_reload(void* argument){
  53     LPC_GPIO_TypeDef *gpios[5] ={LPC_GPIO0,LPC_GPIO1,LPC_GPIO2,LPC_GPIO3,LPC_GPIO4};
  54     this->microseconds_per_step_pulse   =  this->kernel->config->get(microseconds_per_step_pulse_ckeckusm  );
  55     this->acceleration_ticks_per_second =  this->kernel->config->get(acceleration_ticks_per_second_checksum);
  56     this->minimum_steps_per_minute      =  this->kernel->config->get(minimum_steps_per_minute_checksum     );
  57     this->base_stepping_frequency       =  this->kernel->config->get(base_stepping_frequency_checksum      );
  58     this->step_gpio_port      = gpios[(int)this->kernel->config->get(step_gpio_port_checksum               )];
  59     this->dir_gpio_port       = gpios[(int)this->kernel->config->get(dir_gpio_port_checksum                )];
  60     this->alpha_step_pin                =  this->kernel->config->get(alpha_step_pin_checksum               );
  61     this->beta_step_pin                 =  this->kernel->config->get(beta_step_pin_checksum                );
  62     this->gamma_step_pin                =  this->kernel->config->get(gamma_step_pin_checksum               );
  63     this->alpha_dir_pin                 =  this->kernel->config->get(alpha_dir_pin_checksum                );
  64     this->beta_dir_pin                  =  this->kernel->config->get(beta_dir_pin_checksum                 );
  65     this->gamma_dir_pin                 =  this->kernel->config->get(gamma_dir_pin_checksum                );
  66     this->step_mask = ( 1 << this->alpha_step_pin ) + ( 1 << this->beta_step_pin ) + ( 1 << this->gamma_step_pin );
  67     this->dir_mask  = ( 1 << this->alpha_dir_pin  ) + ( 1 << this->beta_dir_pin  ) + ( 1 << this->gamma_dir_pin  );
  68     this->step_bits[ALPHA_STEPPER ] = this->alpha_step_pin;
  69     this->step_bits[BETA_STEPPER  ] = this->beta_step_pin;
  70     this->step_bits[GAMMA_STEPPER ] = this->gamma_step_pin;
  71
  72     // Set the Timer interval for Match Register 1,
  73     LPC_TIM0->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
  74 }
  75
  76
  77 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
  78 void Stepper::on_pause(void* argument){
  79     LPC_TIM0->TCR = 0;
  80     this->acceleration_ticker.detach();
  81 }
  82
  83 // When the play/pause button is set to play, or a module calls the ON_PLAY event
  84 void Stepper::on_play(void* argument){
  85     LPC_TIM0->TCR = 1;
  86     this->acceleration_ticker.attach_us(this, &Stepper::trapezoid_generator_tick, 1000000/this->acceleration_ticks_per_second);
  87 }
  88
  89
  90 // Timer0 ISR
  91 // MR0 is used to call the main stepping interrupt, and MR1 to reset the stepping pins
  92 extern "C" void TIMER0_IRQHandler (void){
  93     if((LPC_TIM0->IR >> 1) & 1){
  94         LPC_TIM0->IR |= 1 << 1;
  95         stepper->step_gpio_port->FIOCLR = stepper->step_mask;
  96     }
  97     if((LPC_TIM0->IR >> 0) & 1){
  98         LPC_TIM0->IR |= 1 << 0;
  99         stepper->main_interrupt();
 100     }
 101 }
 102
 103
 104 //Called ( apriori only by the planner ) when Stepper asked to wake up : means we have work to do. Argument is the queue so that we can use it. //TODO : Get rid of this
 105 void Stepper::on_stepper_wake_up(void* argument){
 106 }
 107
 108 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Smoothie. It is executed at the rate set with
 109 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 110 void Stepper::main_interrupt(){
 111
 112     // Step dir pins first, then step pinse, stepper drivers like to know the direction before the step signal comes in
 113     this->dir_gpio_port->FIOCLR  =                     this->dir_mask;
 114     this->dir_gpio_port->FIOSET  =  this->out_bits &   this->dir_mask;
 115     this->step_gpio_port->FIOSET =  this->out_bits &   this->step_mask;
 116
 117     // If there is no current block, attempt to pop one from the buffer, if there is none, go to sleep, if there is none, go to sleep
 118     if( this->current_block == NULL ){
 119         this->current_block = this->kernel->planner->get_current_block();
 120         if( this->current_block != NULL ){
 121             if( this->current_block->computed == false ){
 122                this->current_block = NULL;
 123             }else{
 124                 this->trapezoid_generator_reset();
 125                 this->update_offsets();
 126                 for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
 127                 this->step_events_completed = 0;
 128                 this->kernel->call_event(ON_BLOCK_BEGIN, this->current_block);
 129                 //this->kernel->planner->dump_queue();
 130             }
 131         }else{
 132             // Go to sleep
 133             //LPC_TIM0->MR0 = 10000;
 134             //LPC_TIM0->MR1 = 500;
 135             //LPC_TIM0->TCR = 0;
 136         }
 137     }
 138
 139     if( this->current_block != NULL ){
 140         // Set bits for direction and steps
 141         this->out_bits = this->current_block->direction_bits;
 142         for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
 143             this->counters[stpr] += this->counter_increment; // (1<<16)>>this->divider; // Basically += 1/divider if we were in floating point arythmetic, but here the counters precision is 1/(2^16).
 144             if( this->counters[stpr] > this->offsets[stpr] && this->stepped[stpr] < this->current_block->steps[stpr] ){
 145                 this->counters[stpr] -= this->offsets[stpr] ;
 146                 this->stepped[stpr]++;
 147                 this->out_bits |= (1 << this->step_bits[stpr]);
 148             }
 149         }
 150         // If current block is finished, reset pointer
 151         this->step_events_completed += this->counter_increment; // (1<<16)>>this->divider; // /this->divider;
 152         if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){
 153             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] ){
 154                 this->kernel->call_event(ON_BLOCK_END, this->current_block);
 155                 this->current_block->pop_and_execute_gcode(this->kernel);
 156                 this->current_block = NULL;
 157                 this->kernel->planner->discard_current_block();
 158             }
 159         }
 160
 161     }else{
 162         this->out_bits = 0;
 163     }
 164 }
 165
 166 void Stepper::update_offsets(){
 167     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
 168         this->offsets[stpr] = (int)floor((float)((float)(1<<16)*(float)((float)this->current_block->steps_event_count / (float)this->current_block->steps[stpr])));
 169     }
 170 }
 171
 172
 173 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 174 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 175 // current_block stays untouched by outside handlers for the duration of this function call.
 176 void Stepper::trapezoid_generator_tick() {
 177     if(this->current_block && !this->kernel->planner->computing ) {
 178       if(this->step_events_completed < this->current_block->accelerate_until<<16) {
 179           this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 180           if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 181               this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 182           }
 183           this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 184       } else if (this->step_events_completed > this->current_block->decelerate_after<<16) {
 185           // NOTE: We will only reduce speed if the result will be > 0. This catches small
 186           // rounding errors that might leave steps hanging after the last trapezoid tick.
 187           if (this->trapezoid_adjusted_rate > this->current_block->rate_delta) {
 188               this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 189           }
 190           if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 191               this->trapezoid_adjusted_rate = this->current_block->final_rate;
 192           }
 193           this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 194       } else {
 195           // Make sure we cruise at exactly nominal rate
 196           if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
 197               this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 198               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 199           }
 200       }
 201   }
 202 }
 203
 204
 205 // Initializes the trapezoid generator from the current block. Called whenever a new
 206 // block begins.
 207 void Stepper::trapezoid_generator_reset(){
 208     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 209     this->trapezoid_tick_cycle_counter = 0;
 210     this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 211     this->trapezoid_generator_busy = false;
 212 }
 213
 214 void Stepper::set_step_events_per_minute( double steps_per_minute ){
 215
 216     // We do not step slower than this
 217     if( steps_per_minute < this->minimum_steps_per_minute ){ steps_per_minute = this->minimum_steps_per_minute; } //TODO: Get from config
 218
 219     // The speed factor is the factor by which we must multiply the minimal step frequency to reach the maximum step frequency
 220     // The higher, the smoother the movement will be, but the closer the maximum and minimum frequencies are, the smaller the factor is
 221     double speed_factor = this->base_stepping_frequency / (steps_per_minute/60L); //TODO: Get from config
 222     if( speed_factor < 1 ){ speed_factor=1; }
 223     this->counter_increment = int(floor((1<<16)/speed_factor));
 224
 225     // Set the Timer interval
 226     LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * speed_factor ) );
 227
 228     // In case we change the Match Register to a value the Timer Counter has past
 229     if( LPC_TIM0->TC >= LPC_TIM0->MR0 ){ LPC_TIM0->TCR = 3; LPC_TIM0->TCR = 1; }
 230
 231     this->kernel->call_event(ON_SPEED_CHANGE, this);
 232
 233 }
 234