src/libs/StepTicker.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
   9 #include "StepTicker.h"
  10
  11 #include "libs/nuts_bolts.h"
  12 #include "libs/Module.h"
  13 #include "libs/Kernel.h"
  14 #include "StepperMotor.h"
  15 #include "StreamOutputPool.h"
  16 #include "system_LPC17xx.h" // mbed.h lib
  17 #include <math.h>
  18 #include <mri.h>
  19
  20 extern bool _isr_context;
  21
  22 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
  23 // It has a list of those, and calls their tick() functions at regular intervals
  24 // They then do Bresenham stuff themselves
  25
  26 StepTicker* StepTicker::global_step_ticker;
  27
  28 StepTicker::StepTicker(int nmotors){
  29     StepTicker::global_step_ticker = this;
  30
  31     // Configure the timer
  32     LPC_TIM0->MR0 = 10000000;       // Initial dummy value for Match Register
  33     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
  34     LPC_TIM0->TCR = 0;              // Disable interrupt
  35
  36     LPC_SC->PCONP |= (1 << 2);      // Power Ticker ON
  37     LPC_TIM1->MR0 = 1000000;
  38     LPC_TIM1->MCR = 1;
  39     LPC_TIM1->TCR = 1;              // Enable interrupt
  40
  41     // Default start values
  42     this->a_move_finished = false;
  43     this->reset_step_pins = false;
  44     this->set_frequency(0.001);
  45     this->set_reset_delay(100);
  46     this->last_duration = 0;
  47     //this->overruns= 0;
  48     this->num_motors= nmotors;
  49     this->active_motors= new StepperMotor*[num_motors];
  50     for (int i = 0; i < num_motors; i++){
  51         this->active_motors[i] = nullptr;
  52     }
  53     this->active_motor_bm = 0;
  54
  55     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  56     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  57 }
  58
  59 StepTicker::~StepTicker() {
  60     delete[] this->active_motors;
  61 }
  62
  63 // Set the base stepping frequency
  64 void StepTicker::set_frequency( float frequency ){
  65     this->frequency = frequency;
  66     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
  67     LPC_TIM0->MR0 = this->period;
  68     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  69         LPC_TIM0->TCR = 3;  // Reset
  70         LPC_TIM0->TCR = 1;  // Reset
  71     }
  72 }
  73
  74 // Set the reset delay
  75 void StepTicker::set_reset_delay( float seconds ){
  76     this->delay = int(floor(float(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
  77     LPC_TIM1->MR0 = this->delay;
  78 }
  79
  80 // Call tick() on each active motor
  81 inline void StepTicker::tick(){
  82     _isr_context = true;
  83     int i;
  84     uint32_t bm = 1;
  85     // We iterate over each active motor
  86     for (i = 0; i < num_motors; i++, bm <<= 1){
  87         if (this->active_motor_bm & bm){
  88             this->active_motors[i]->tick();
  89         }
  90     }
  91     _isr_context = false;
  92 }
  93
  94 // Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
  95 // all tick()s are called before we do the move finishing
  96 void StepTicker::signal_a_move_finished(){
  97     _isr_context = true;
  98
  99     uint16_t bitmask = 1;
 100     for ( uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){
 101         if (this->active_motor_bm & bitmask){
 102             if(this->active_motors[motor]->is_move_finished){
 103                 this->active_motors[motor]->signal_move_finished();
 104                 if(this->active_motors[motor]->moving == false){
 105                     if (motor > 0){
 106                         motor--;
 107                         bitmask >>= 1;
 108                     }
 109                 }
 110             }
 111         }
 112     }
 113     this->a_move_finished = false;
 114
 115     _isr_context = false;
 116 }
 117
 118 // Reset step pins on all active motors
 119 inline void StepTicker::reset_tick(){
 120     _isr_context = true;
 121
 122     int i;
 123     uint32_t bm;
 124     for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
 125     {
 126         if (this->active_motor_bm & bm)
 127             this->active_motors[i]->unstep();
 128     }
 129
 130     _isr_context = false;
 131 }
 132
 133 extern "C" void TIMER1_IRQHandler (void){
 134     LPC_TIM1->IR |= 1 << 0;
 135     StepTicker::global_step_ticker->reset_tick();
 136 }
 137
 138 // The actual interrupt handler where we do all the work
 139 extern "C" void TIMER0_IRQHandler (void){
 140     StepTicker::global_step_ticker->TIMER0_IRQHandler();
 141 }
 142
 143 void StepTicker::TIMER0_IRQHandler (void){
 144     // Reset interrupt register
 145     LPC_TIM0->IR |= 1 << 0;
 146
 147     // Step pins
 148     uint16_t bitmask = 1;
 149     for (uint8_t motor = 0; motor < num_motors; motor++, bitmask <<= 1){
 150         if (this->active_motor_bm & bitmask){
 151             this->active_motors[motor]->tick();
 152         }
 153     }
 154
 155     // We may have set a pin on in this tick, now we start the timer to set it off
 156     if( this->reset_step_pins ){
 157         LPC_TIM1->TCR = 3;
 158         LPC_TIM1->TCR = 1;
 159         this->reset_step_pins = false;
 160     }else{
 161         // Nothing happened, nothing after this really matters
 162         // TODO : This could be a problem when we use Actuators instead of StepperMotors, because this flag is specific to step generation
 163         LPC_TIM0->MR0 = this->period;
 164         return;
 165     }
 166
 167     // If a move finished in this tick, we have to tell the actuator to act accordingly
 168     if( this->a_move_finished ){
 169
 170         // Do not get out of here before everything is nice and tidy
 171         LPC_TIM0->MR0 = 20000000;
 172
 173         this->signal_a_move_finished();
 174
 175         // If we went over the duration an interrupt is supposed to last, we have a problem
 176         // That can happen typically when we change blocks, where more than usual computation is done
 177         // This can be OK, if we take notice of it, which we do now
 178         if( LPC_TIM0->TC > this->period ){ // TODO: remove the size condition
 179             //overruns++;
 180
 181             uint32_t start_tc = LPC_TIM0->TC;
 182
 183             // How many ticks we want to skip ( this does not include the current tick, but we add the time we spent doing this computation last time )
 184             uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + this->last_duration ) / this->period );
 185
 186             // Next step is now to reduce this to how many steps we can *actually* skip, as we do not want to cause an actual step
 187             uint32_t ticks_we_actually_can_skip = ticks_to_skip;
 188
 189             int i;
 190             uint32_t bm;
 191             for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
 192             {
 193                 if((this->active_motor_bm & bm) != 0) {
 194                     if(this->active_motors[i]->fx_ticks_per_step > this->active_motors[i]->fx_counter) {
 195                         ticks_we_actually_can_skip =
 196                             min(ticks_we_actually_can_skip, (uint32_t)((active_motors[i]->fx_ticks_per_step - active_motors[i]->fx_counter) >> active_motors[i]->fx_shift));
 197                     }else{
 198                         ticks_we_actually_can_skip= 0;
 199                     }
 200                 }
 201             }
 202
 203             // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
 204             for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
 205             {
 206                 if (this->active_motor_bm & bm)
 207                     this->active_motors[i]->fx_counter += ((uint64_t)ticks_we_actually_can_skip << active_motors[i]->fx_shift);
 208             }
 209
 210             // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime )
 211             LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * this->period;
 212
 213             // This is so that we know how long this computation takes, and we can take it into account next time
 214             int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
 215             if( difference > 0 ){ this->last_duration = (uint32_t)difference; }
 216
 217         }else{
 218             LPC_TIM0->MR0 = this->period;
 219         }
 220
 221         while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
 222             LPC_TIM0->MR0 += this->period;
 223         }
 224
 225     }
 226
 227 }
 228
 229
 230 // We make a list of steppers that want to be called so that we don't call them for nothing
 231 void StepTicker::add_motor_to_active_list(StepperMotor* motor)
 232 {
 233     uint32_t bm;
 234     int i;
 235     for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
 236     {
 237         if (this->active_motors[i] == motor)
 238         {
 239             this->active_motor_bm |= bm;
 240             if( this->active_motor_bm != 0 ){
 241                 LPC_TIM0->TCR = 1;               // Enable interrupt
 242             }
 243             return;
 244         }
 245         if (this->active_motors[i] == nullptr)
 246         {
 247             this->active_motors[i] = motor;
 248             this->active_motor_bm |= bm;
 249             if( this->active_motor_bm != 0 ){
 250                 LPC_TIM0->TCR = 1;               // Enable interrupt
 251             }
 252             return;
 253         }
 254     }
 255     return;
 256 }
 257
 258 // Remove a stepper from the list of active motors
 259 void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
 260 {
 261     uint32_t bm; int i;
 262     for (i = 0, bm = 1; i < num_motors; i++, bm <<= 1)
 263     {
 264         if (this->active_motors[i] == motor)
 265         {
 266             this->active_motor_bm &= ~bm;
 267             // If we have no motor to work on, disable the whole interrupt
 268             if( this->active_motor_bm == 0 ){
 269                 LPC_TIM0->TCR = 0;               // Disable interrupt
 270             }
 271             return;
 272         }
 273     }
 274 }