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     this->active_motors[0] = NULL;
  46
  47     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  48     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  49 }
  50
  51 // Set the base stepping frequency
  52 void StepTicker::set_frequency( double frequency ){
  53     this->frequency = frequency;
  54     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
  55     LPC_TIM0->MR0 = this->period;
  56     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  57         LPC_TIM0->TCR = 3;  // Reset
  58         LPC_TIM0->TCR = 1;  // Reset
  59     }
  60 }
  61
  62 // Set the reset delay
  63 void StepTicker::set_reset_delay( double seconds ){
  64     this->delay = int(floor(double(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
  65     LPC_TIM1->MR0 = this->delay;
  66 }
  67
  68 // Add a stepper motor object to our list of steppers we must take care of
  69 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
  70     this->stepper_motors.push_back(stepper_motor);
  71     stepper_motor->step_ticker = this;
  72     this->has_axes = true;
  73     return stepper_motor;
  74 }
  75
  76 // Call tick() on each active motor
  77 inline void StepTicker::tick(){
  78     uint8_t current_id = 0;
  79     StepperMotor* current = this->active_motors[0];
  80     while(current != NULL ){
  81         current->tick();
  82         current_id++;
  83         current = this->active_motors[current_id];
  84     }
  85 }
  86
  87 // Call signal_mode_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
  88 // all tick()s are called before we do the move finishing
  89 void StepTicker::signal_moves_finished(){
  90     uint8_t current_id = 0;
  91     StepperMotor* current = this->active_motors[0];
  92     while(current != NULL ){
  93         if( current->is_move_finished ){
  94             current->signal_move_finished();
  95             if( current->moving == false ){ current_id--; }
  96         }
  97         current_id++;
  98         current = this->active_motors[current_id];
  99     }
 100     this->moves_finished = false;
 101 }
 102
 103 // Reset step pins on all active motors
 104 inline void StepTicker::reset_tick(){
 105     uint8_t current_id = 0;
 106     StepperMotor* current = this->active_motors[0];
 107     while(current != NULL ){
 108         current->step_pin->set(0);
 109         current_id++;
 110         current = this->active_motors[current_id];
 111     }
 112 }
 113
 114 extern "C" void TIMER1_IRQHandler (void){
 115     LPC_TIM1->IR |= 1 << 0;
 116     global_step_ticker->reset_tick();
 117 }
 118
 119
 120 //#pragma GCC push_options
 121 //#pragma GCC optimize ("O0")
 122
 123
 124 // The actual interrupt handler where we do all the work
 125 extern "C" void TIMER0_IRQHandler (void){
 126
 127     LPC_GPIO1->FIODIR |= 1<<18;
 128     LPC_GPIO1->FIOSET = 1<<18;
 129
 130     uint32_t initial_tc = LPC_TIM0->TC;
 131
 132     LPC_TIM0->IR |= 1 << 0;
 133
 134     // If no axes enabled, just ignore for now
 135     if( global_step_ticker->active_motors[0] == NULL ){
 136         LPC_GPIO1->FIOCLR = 1<<18;
 137         return;
 138     }
 139
 140     // Do not get out of here before everything is nice and tidy
 141     LPC_TIM0->MR0 = 2000000;
 142
 143     // Step pins
 144     global_step_ticker->tick();
 145
 146     // We may have set a pin on in this tick, now we start the timer to set it off
 147     if( global_step_ticker->reset_step_pins ){
 148         LPC_TIM1->TCR = 3;
 149         LPC_TIM1->TCR = 1;
 150         global_step_ticker->reset_step_pins = false;
 151     }
 152
 153     // If a move finished in this tick, we have to tell the actuator to act accordingly
 154     if( global_step_ticker->moves_finished ){ global_step_ticker->signal_moves_finished(); }
 155
 156     uint32_t after_signal = LPC_TIM0->TC;
 157
 158     // If we went over the duration an interrupt is supposed to last, we have a problem
 159     // That can happen tipically when we change blocks, where more than usual computation is done
 160     // This can be OK, if we take notice of it, which we do now
 161     if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO: remove the size condition
 162
 163         LPC_GPIO1->FIODIR |= 1<<19;
 164         LPC_GPIO1->FIOSET = 1<<19;
 165
 166        uint32_t start_tc = LPC_TIM0->TC;
 167
 168         // 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 )
 169         uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
 170
 171         // Next step is now to reduce this to how many steps we can *actually* skip
 172         uint32_t ticks_we_actually_can_skip = ticks_to_skip;
 173         uint8_t current_id = 0; StepperMotor* current = global_step_ticker->active_motors[0];
 174         while(current != NULL ){   // For each active stepper
 175             ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)current->fx_ticks_per_step - (uint64_t)current->fx_counter ) >> 32) );
 176             current_id++;
 177             current = global_step_ticker->active_motors[current_id];
 178         }
 179
 180         // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
 181         current_id = 0; current = global_step_ticker->active_motors[0];
 182         while(current != NULL ){   // For each active stepper
 183             current->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
 184             current_id++;
 185             current = global_step_ticker->active_motors[current_id];
 186         }
 187
 188         // 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 )
 189         // LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
 190         LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
 191
 192         // This is so that we know how long this computation takes, and we can take it into account next time
 193         int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
 194         if( difference > 0 ){ global_step_ticker->last_duration = (uint32_t)difference; }
 195
 196         //if( global_step_ticker->last_duration > 2000 || LPC_TIM0->MR0 > 2000 || LPC_TIM0->TC > 2000 || initial_tc > 2000 ){ __debugbreak(); }
 197
 198         LPC_GPIO1->FIOCLR = 1<<19;
 199
 200     }else{
 201         LPC_TIM0->MR0 = global_step_ticker->period;
 202     }
 203
 204     LPC_GPIO1->FIOCLR = 1<<18;
 205
 206     while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
 207         LPC_TIM0->MR0 += global_step_ticker->period;
 208     }
 209
 210     LPC_GPIO1->FIOCLR = 1<<18;
 211 }
 212
 213
 214 //#pragma GCC pop_options
 215
 216 // We make a list of steppers that want to be called so that we don't call them for nothing
 217 void StepTicker::add_motor_to_active_list(StepperMotor* motor){
 218     uint8_t current_id = 0;
 219     StepperMotor* current = this->active_motors[0];
 220     while(current != NULL ){
 221         if( current == motor ){
 222             return;  // Motor already in list
 223         }
 224         current_id++;
 225         current = this->active_motors[current_id];
 226     }
 227     this->active_motors[current_id  ] = motor;
 228     this->active_motors[current_id+1] = NULL;
 229
 230 }
 231
 232 // Remove a stepper from the list of active motors
 233 void StepTicker::remove_motor_from_active_list(StepperMotor* motor){
 234     uint8_t current_id = 0;
 235     uint8_t offset = 0;
 236     StepperMotor* current = this->active_motors[0];
 237     while( current != NULL ){
 238         if( current == motor ){ offset++; }
 239         this->active_motors[current_id] = this->active_motors[current_id+offset];
 240         current_id++;
 241         current = this->active_motors[current_id+offset];
 242     }
 243     this->active_motors[current_id] = NULL;
 244 }
 245