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 "libs/Module.h"
   9 #include "libs/Kernel.h"
  10 #include "Stepper.h"
  11 #include "Planner.h"
  12 #include "Player.h"
  13 #include "mbed.h"
  14 #include <vector>
  15 using namespace std;
  16 #include "libs/nuts_bolts.h"
  17
  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_BLOCK_BEGIN);
  30     this->register_for_event(ON_BLOCK_END);
  31     this->register_for_event(ON_PLAY);
  32     this->register_for_event(ON_PAUSE);
  33
  34     // Get onfiguration
  35     this->on_config_reload(this);
  36
  37     // Acceleration ticker
  38     this->kernel->slow_ticker->set_frequency(this->acceleration_ticks_per_second);
  39     this->kernel->slow_ticker->attach( this, &Stepper::trapezoid_generator_tick );
  40
  41     // Initiate main_interrupt timer and step reset timer
  42     LPC_TIM0->MR0 = 10000;
  43     LPC_TIM0->MCR = 11; // for MR0 and MR1, with no reset at MR1
  44     NVIC_EnableIRQ(TIMER0_IRQn);
  45     NVIC_SetPriority(TIMER3_IRQn, 4);
  46     NVIC_SetPriority(TIMER0_IRQn, 1);
  47     NVIC_SetPriority(TIMER2_IRQn, 2);
  48     NVIC_SetPriority(TIMER1_IRQn, 3);
  49     LPC_TIM0->TCR = 1;
  50
  51
  52     // Step and Dir pins as outputs
  53     this->step_gpio_port->FIODIR |= this->step_mask;
  54     this->dir_gpio_port->FIODIR  |= this->dir_mask;
  55
  56 }
  57
  58 // Get configuration from the config file
  59 void Stepper::on_config_reload(void* argument){
  60     LPC_GPIO_TypeDef *gpios[5] ={LPC_GPIO0,LPC_GPIO1,LPC_GPIO2,LPC_GPIO3,LPC_GPIO4};
  61     this->microseconds_per_step_pulse   =  this->kernel->config->value(microseconds_per_step_pulse_ckeckusm  )->by_default(5     )->as_number();
  62     this->acceleration_ticks_per_second =  this->kernel->config->value(acceleration_ticks_per_second_checksum)->by_default(100   )->as_number();
  63     this->minimum_steps_per_minute      =  this->kernel->config->value(minimum_steps_per_minute_checksum     )->by_default(1200  )->as_number();
  64     this->base_stepping_frequency       =  this->kernel->config->value(base_stepping_frequency_checksum      )->by_default(100000)->as_number();
  65     this->step_gpio_port      = gpios[(int)this->kernel->config->value(step_gpio_port_checksum               )->by_default(0     )->as_number()];
  66     this->dir_gpio_port       = gpios[(int)this->kernel->config->value(dir_gpio_port_checksum                )->by_default(0     )->as_number()];
  67     this->alpha_step_pin                =  this->kernel->config->value(alpha_step_pin_checksum               )->by_default(1     )->as_number();
  68     this->beta_step_pin                 =  this->kernel->config->value(beta_step_pin_checksum                )->by_default(2     )->as_number();
  69     this->gamma_step_pin                =  this->kernel->config->value(gamma_step_pin_checksum               )->by_default(3     )->as_number();
  70     this->alpha_dir_pin                 =  this->kernel->config->value(alpha_dir_pin_checksum                )->by_default(4     )->as_number();
  71     this->beta_dir_pin                  =  this->kernel->config->value(beta_dir_pin_checksum                 )->by_default(5     )->as_number();
  72     this->gamma_dir_pin                 =  this->kernel->config->value(gamma_dir_pin_checksum                )->by_default(6     )->as_number();
  73     this->step_mask                     = ( 1 << this->alpha_step_pin ) + ( 1 << this->beta_step_pin ) + ( 1 << this->gamma_step_pin );
  74     this->dir_mask                      = ( 1 << this->alpha_dir_pin  ) + ( 1 << this->beta_dir_pin  ) + ( 1 << this->gamma_dir_pin  );
  75     this->step_bits[ALPHA_STEPPER ]     = this->alpha_step_pin;
  76     this->step_bits[BETA_STEPPER  ]     = this->beta_step_pin;
  77     this->step_bits[GAMMA_STEPPER ]     = this->gamma_step_pin;
  78     this->step_invert_mask = ( this->kernel->config->value( alpha_step_pin_checksum )->is_inverted() << this->alpha_step_pin ) +
  79                              ( this->kernel->config->value( beta_step_pin_checksum  )->is_inverted() << this->beta_step_pin  ) +
  80                              ( this->kernel->config->value( gamma_step_pin_checksum )->is_inverted() << this->gamma_step_pin );
  81     this->dir_invert_mask =  ( this->kernel->config->value( alpha_dir_pin_checksum  )->is_inverted() << this->alpha_dir_pin  ) +
  82                              ( this->kernel->config->value( beta_dir_pin_checksum   )->is_inverted() << this->beta_dir_pin   ) +
  83                              ( this->kernel->config->value( gamma_dir_pin_checksum  )->is_inverted() << this->gamma_dir_pin  );
  84
  85     // Set the Timer interval for Match Register 1,
  86     LPC_TIM0->MR1 = (( SystemCoreClock/4 ) / 1000000 ) * this->microseconds_per_step_pulse;
  87 }
  88
  89 // When the play/pause button is set to pause, or a module calls the ON_PAUSE event
  90 void Stepper::on_pause(void* argument){
  91     LPC_TIM0->TCR = 0;
  92     LPC_TIM2->TCR = 0;
  93 }
  94
  95 // When the play/pause button is set to play, or a module calls the ON_PLAY event
  96 void Stepper::on_play(void* argument){
  97     LPC_TIM0->TCR = 1;
  98     LPC_TIM2->TCR = 1;
  99 }
 100
 101 // A new block is popped from the queue
 102 void Stepper::on_block_begin(void* argument){
 103     Block* block  = static_cast<Block*>(argument);
 104
 105     // The stepper does not care about 0-blocks
 106     if( block->millimeters == 0.0 ){ return; }
 107
 108     // Mark the new block as of interrest to us
 109     block->take();
 110
 111     // Setup
 112     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){ this->counters[stpr] = 0; this->stepped[stpr] = 0; }
 113     this->step_events_completed = 0;
 114
 115     this->current_block = block;
 116
 117     // This is just to save computing power and not do it every step
 118     this->update_offsets();
 119
 120     // Setup acceleration for this block
 121     this->trapezoid_generator_reset();
 122
 123 }
 124
 125 // Current block is discarded
 126 void Stepper::on_block_end(void* argument){
 127     Block* block  = static_cast<Block*>(argument);
 128     LPC_TIM0->MR0 = 1000000;
 129     this->current_block = NULL; //stfu !
 130 }
 131
 132 // Timer0 ISR
 133 // MR0 is used to call the main stepping interrupt, and MR1 to reset the stepping pins
 134 extern "C" void TIMER0_IRQHandler (void){
 135     if((LPC_TIM0->IR >> 1) & 1){
 136         LPC_TIM0->IR |= 1 << 1;
 137         // Clear step pins
 138         stepper->step_gpio_port->FIOSET =  stepper->step_invert_mask & stepper->step_mask;
 139         stepper->step_gpio_port->FIOCLR = ~stepper->step_invert_mask & stepper->step_mask;
 140     }
 141     if((LPC_TIM0->IR >> 0) & 1){
 142         LPC_TIM0->IR |= 1 << 0;
 143         stepper->main_interrupt();
 144     }
 145 }
 146
 147 // "The Stepper Driver Interrupt" - This timer interrupt is the workhorse of Smoothie. It is executed at the rate set with
 148 // config_step_timer. It pops blocks from the block_buffer and executes them by pulsing the stepper pins appropriately.
 149 inline void Stepper::main_interrupt(){
 150
 151     // TODO: Explain this magic
 152     // Step dir pins first, then step pinse, stepper drivers like to know the direction before the step signal comes in
 153     // Clear dir   pins
 154     this->dir_gpio_port->FIOSET  =                          this->dir_invert_mask          & this->dir_mask;
 155     this->dir_gpio_port->FIOCLR  =     ~                    this->dir_invert_mask          & this->dir_mask;
 156     // Set   dir   pins
 157     this->dir_gpio_port->FIOSET  =   (     this->out_bits ^ this->dir_invert_mask    )     & this->dir_mask;
 158     this->dir_gpio_port->FIOCLR  =   ( ~ ( this->out_bits ^ this->dir_invert_mask  ) )     & this->dir_mask;
 159     // Set   step  pins
 160     this->step_gpio_port->FIOSET =   (     this->out_bits ^ this->step_invert_mask   )     & this->step_mask;
 161     this->step_gpio_port->FIOCLR =   ( ~ ( this->out_bits ^ this->step_invert_mask ) )     & this->step_mask;
 162
 163     if( this->current_block != NULL ){
 164         // Set bits for direction and steps
 165         this->out_bits = this->current_block->direction_bits;
 166         for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
 167             this->counters[stpr] += this->counter_increment;
 168             if( this->counters[stpr] > this->offsets[stpr] && this->stepped[stpr] < this->current_block->steps[stpr] ){
 169                 this->counters[stpr] -= this->offsets[stpr] ;
 170                 this->stepped[stpr]++;
 171                 this->out_bits |= (1 << this->step_bits[stpr]);
 172             }
 173         }
 174         // If current block is finished, reset pointer
 175         this->step_events_completed += this->counter_increment;
 176         if( this->step_events_completed >= this->current_block->steps_event_count<<16 ){
 177             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] ){
 178                 if( this->current_block != NULL ){
 179                     this->current_block->release();
 180                 }
 181             }
 182         }
 183     }else{
 184         this->out_bits = 0;
 185     }
 186
 187 }
 188
 189 // We compute this here instead of each time in the interrupt
 190 void Stepper::update_offsets(){
 191     for( int stpr=ALPHA_STEPPER; stpr<=GAMMA_STEPPER; stpr++){
 192         this->offsets[stpr] = (int)floor((float)((float)(1<<16)*(float)((float)this->current_block->steps_event_count / (float)this->current_block->steps[stpr])));
 193     }
 194 }
 195
 196 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 197 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 198 // current_block stays untouched by outside handlers for the duration of this function call.
 199 void Stepper::trapezoid_generator_tick() {
 200     if(this->current_block && !this->trapezoid_generator_busy ) {
 201           if(this->step_events_completed < this->current_block->accelerate_until<<16) {
 202               this->trapezoid_adjusted_rate += this->current_block->rate_delta;
 203               if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
 204                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 205               }
 206               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 207           } else if (this->step_events_completed >= this->current_block->decelerate_after<<16) {
 208               // NOTE: We will only reduce speed if the result will be > 0. This catches small
 209               // rounding errors that might leave steps hanging after the last trapezoid tick.
 210               if (this->trapezoid_adjusted_rate > double(this->current_block->rate_delta) * 1.5) {
 211                   this->trapezoid_adjusted_rate -= this->current_block->rate_delta;
 212               }else{
 213                   this->trapezoid_adjusted_rate = floor(double(this->trapezoid_adjusted_rate / 2 ));
 214               }
 215               if (this->trapezoid_adjusted_rate < this->current_block->final_rate ) {
 216                   this->trapezoid_adjusted_rate = this->current_block->final_rate;
 217               }
 218               this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 219           } else {
 220               // Make sure we cruise at exactly nominal rate
 221               if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
 222                   this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
 223                   this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 224               }
 225           }
 226     }
 227 }
 228
 229 // Initializes the trapezoid generator from the current block. Called whenever a new
 230 // block begins.
 231 void Stepper::trapezoid_generator_reset(){
 232     this->trapezoid_adjusted_rate = this->current_block->initial_rate;
 233     this->trapezoid_tick_cycle_counter = 0;
 234     // 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
 235     this->trapezoid_generator_busy = true;
 236     this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
 237     this->trapezoid_generator_busy = false;
 238 }
 239
 240 void Stepper::set_step_events_per_minute( double steps_per_minute ){
 241
 242     // We do not step slower than this
 243     if( steps_per_minute < this->minimum_steps_per_minute ){ steps_per_minute = this->minimum_steps_per_minute; }
 244
 245     // The speed factor is the factor by which we must multiply the minimal step frequency to reach the maximum step frequency
 246     // The higher, the smoother the movement will be, but the closer the maximum and minimum frequencies are, the smaller the factor is
 247     double speed_factor = this->base_stepping_frequency / (steps_per_minute/60L);
 248     if( speed_factor < 1 ){ speed_factor=1; }
 249     this->counter_increment = int(floor((1<<16)/floor(speed_factor)));
 250
 251     // Set the Timer interval
 252     LPC_TIM0->MR0 = floor( ( SystemCoreClock/4 ) / ( (steps_per_minute/60L) * speed_factor ) );
 253
 254     if( LPC_TIM0->MR0 < 150 ){
 255         LPC_TIM0->MR0 = 150;
 256         this->kernel->serial->printf("tim0mr0: %d, steps_per minute: %f \r\n", LPC_TIM0->MR0, steps_per_minute );
 257     }
 258
 259     // In case we change the Match Register to a value the Timer Counter has past
 260     if( LPC_TIM0->TC >= LPC_TIM0->MR0 ){ LPC_TIM0->TCR = 3; LPC_TIM0->TCR = 1; }
 261
 262     this->kernel->call_event(ON_SPEED_CHANGE, this);
 263
 264 }
 265