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