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