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 using namespace std;
  12 #include <vector>
  13
  14 #include "libs/nuts_bolts.h"
  15 #include "libs/Module.h"
  16 #include "libs/Kernel.h"
  17 #include "StepperMotor.h"
  18
  19 #include "system_LPC17xx.h" // mbed.h lib
  20 #include <math.h>
  21 #include <mri.h>
  22
  23 extern bool _isr_context;
  24
  25 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
  26 // It has a list of those, and calls their tick() functions at regular intervals
  27 // They then do Bresenham stuff themselves
  28
  29 StepTicker* StepTicker::global_step_ticker;
  30
  31 StepTicker::StepTicker(){
  32     StepTicker::global_step_ticker = this;
  33
  34     // Configure the timer
  35     LPC_TIM0->MR0 = 10000000;       // Initial dummy value for Match Register
  36     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
  37     LPC_TIM0->TCR = 0;              // Disable interrupt
  38
  39     LPC_SC->PCONP |= (1 << 2);      // Power Ticker ON
  40     LPC_TIM1->MR0 = 1000000;
  41     LPC_TIM1->MCR = 1;
  42     LPC_TIM1->TCR = 1;              // Enable interrupt
  43
  44     // Default start values
  45     this->moves_finished = false;
  46     this->reset_step_pins = false;
  47     this->debug = 0;
  48     this->has_axes = 0;
  49     this->set_frequency(0.001);
  50     this->set_reset_delay(100);
  51     this->last_duration = 0;
  52     for (int i = 0; i < 12; i++){
  53         this->active_motors[i] = NULL;
  54     }
  55     this->active_motor_bm = 0;
  56
  57     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  58     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  59 }
  60
  61 // Set the base stepping frequency
  62 void StepTicker::set_frequency( float frequency ){
  63     this->frequency = frequency;
  64     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
  65     LPC_TIM0->MR0 = this->period;
  66     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  67         LPC_TIM0->TCR = 3;  // Reset
  68         LPC_TIM0->TCR = 1;  // Reset
  69     }
  70 }
  71
  72 // Set the reset delay
  73 void StepTicker::set_reset_delay( float seconds ){
  74     this->delay = int(floor(float(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
  75     LPC_TIM1->MR0 = this->delay;
  76 }
  77
  78 // Add a stepper motor object to our list of steppers we must take care of
  79 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
  80     this->stepper_motors.push_back(stepper_motor);
  81     stepper_motor->step_ticker = this;
  82     this->has_axes = true;
  83     return stepper_motor;
  84 }
  85
  86 // Call tick() on each active motor
  87 inline void StepTicker::tick(){
  88     _isr_context = true;
  89     int i;
  90     uint32_t bm = 1;
  91     // We iterate over each active motor
  92     for (i = 0; i < 12; i++, bm <<= 1){
  93         if (this->active_motor_bm & bm){
  94             this->active_motors[i]->tick();
  95         }
  96     }
  97     _isr_context = false;
  98 }
  99
 100 // Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
 101 // all tick()s are called before we do the move finishing
 102 void StepTicker::signal_moves_finished(){
 103     _isr_context = true;
 104
 105     uint16_t bitmask = 1;
 106     for ( uint8_t motor = 0; motor < 12; motor++, bitmask <<= 1){
 107         if (this->active_motor_bm & bitmask){
 108             if(this->active_motors[motor]->is_move_finished){
 109                 this->active_motors[motor]->signal_move_finished();
 110                 if(this->active_motors[motor]->moving == false){
 111                     if (motor > 0){
 112                         motor--;
 113                         bitmask >>= 1;
 114                     }
 115                 }
 116             }
 117         }
 118     }
 119     this->moves_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 < 12; 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 < 12; 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->moves_finished ){
 175
 176         // Do not get out of here before everything is nice and tidy
 177         LPC_TIM0->MR0 = 20000000;
 178
 179         this->signal_moves_finished();
 180
 181         // If we went over the duration an interrupt is supposed to last, we have a problem
 182         // That can happen tipically when we change blocks, where more than usual computation is done
 183         // This can be OK, if we take notice of it, which we do now
 184         if( LPC_TIM0->TC > this->period ){ // TODO: remove the size condition
 185
 186             uint32_t start_tc = LPC_TIM0->TC;
 187
 188             // 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 )
 189             uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + this->last_duration ) / this->period );
 190
 191             // Next step is now to reduce this to how many steps we can *actually* skip
 192             uint32_t ticks_we_actually_can_skip = ticks_to_skip;
 193
 194             int i;
 195             uint32_t bm;
 196             for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
 197             {
 198                 if (this->active_motor_bm & bm)
 199                     ticks_we_actually_can_skip =
 200                         min(ticks_we_actually_can_skip,
 201                             (uint32_t)((uint64_t)( (uint64_t)this->active_motors[i]->fx_ticks_per_step - (uint64_t)this->active_motors[i]->fx_counter ) >> 32)
 202                             );
 203             }
 204
 205             // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
 206             for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
 207             {
 208                 if (this->active_motor_bm & bm)
 209                     this->active_motors[i]->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
 210             }
 211
 212             // 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 )
 213             LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * this->period;
 214
 215             // This is so that we know how long this computation takes, and we can take it into account next time
 216             int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
 217             if( difference > 0 ){ this->last_duration = (uint32_t)difference; }
 218
 219         }else{
 220             LPC_TIM0->MR0 = this->period;
 221         }
 222
 223         while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
 224             LPC_TIM0->MR0 += this->period;
 225         }
 226
 227     }
 228
 229 }
 230
 231
 232 // We make a list of steppers that want to be called so that we don't call them for nothing
 233 void StepTicker::add_motor_to_active_list(StepperMotor* motor)
 234 {
 235     uint32_t bm;
 236     int i;
 237     for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
 238     {
 239         if (this->active_motors[i] == motor)
 240         {
 241             this->active_motor_bm |= bm;
 242             if( this->active_motor_bm != 0 ){
 243                 LPC_TIM0->TCR = 1;               // Enable interrupt
 244             }
 245             return;
 246         }
 247         if (this->active_motors[i] == NULL)
 248         {
 249             this->active_motors[i] = motor;
 250             this->active_motor_bm |= bm;
 251             if( this->active_motor_bm != 0 ){
 252                 LPC_TIM0->TCR = 1;               // Enable interrupt
 253             }
 254             return;
 255         }
 256     }
 257     return;
 258 }
 259
 260 // Remove a stepper from the list of active motors
 261 void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
 262 {
 263     uint32_t bm; int i;
 264     for (i = 0, bm = 1; i < 12; i++, bm <<= 1)
 265     {
 266         if (this->active_motors[i] == motor)
 267         {
 268             this->active_motor_bm &= ~bm;
 269             // If we have no motor to work on, disable the whole interrupt
 270             if( this->active_motor_bm == 0 ){
 271                 LPC_TIM0->TCR = 0;               // Disable interrupt
 272             }
 273             return;
 274         }
 275     }
 276 }