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 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
  19 // It has a list of those, and calls their tick() functions at regular intervals
  20 // They then do Bresenham stuff themselves
  21
  22 StepTicker* global_step_ticker;
  23
  24 StepTicker::StepTicker(){
  25     global_step_ticker = this;
  26     LPC_TIM0->MR0 = 10000000;        // Initial dummy value for Match Register
  27     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
  28     LPC_TIM0->TCR = 1;               // Enable interrupt
  29
  30     LPC_SC->PCONP |= (1 << 2);     // Power Ticker ON
  31     LPC_TIM1->MR0 = 1000000;
  32     LPC_TIM1->MCR = 1;
  33     LPC_TIM1->TCR = 1;               // Enable interrupt
  34
  35     // Default start values
  36     this->debug = 0;
  37     this->has_axes = 0;
  38     this->set_frequency(0.001);
  39     this->set_reset_delay(100);
  40     this->last_duration = 0;
  41     this->active_motors[0] = NULL;
  42
  43     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  44     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  45 }
  46
  47 // Set the base stepping frequency
  48 void StepTicker::set_frequency( double frequency ){
  49     this->frequency = frequency;
  50     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
  51     LPC_TIM0->MR0 = this->period;
  52     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  53         LPC_TIM0->TCR = 3;  // Reset
  54         LPC_TIM0->TCR = 1;  // Reset
  55     }
  56 }
  57
  58 // Set the reset delay
  59 void StepTicker::set_reset_delay( double seconds ){
  60     this->delay = int(floor(double(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
  61     LPC_TIM1->MR0 = this->delay;
  62 }
  63
  64 // Add a stepper motor object to our list of steppers we must take care of
  65 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
  66     this->stepper_motors.push_back(stepper_motor);
  67     stepper_motor->step_ticker = this;
  68     this->has_axes = true;
  69     return stepper_motor;
  70 }
  71
  72 inline void StepTicker::tick(){
  73     uint8_t current_id = 0;
  74     StepperMotor* current = this->active_motors[0];
  75     while(current != NULL ){
  76         current->tick();
  77         current_id++;
  78         current = this->active_motors[current_id];
  79     }
  80 }
  81
  82 inline void StepTicker::reset_tick(){
  83
  84     uint8_t current_id = 0;
  85     StepperMotor* current = this->active_motors[0];
  86     while(current != NULL ){
  87         current->step_pin->set(0);
  88         if( current->exit_tick ){
  89             if( current->remove_from_active_list_next_tick ){
  90                 current->remove_from_active_list_next_tick = false;
  91             }else{
  92                 this->remove_motor_from_active_list(current);
  93                 current_id--;
  94             }
  95         }
  96         current_id++;
  97         current = this->active_motors[current_id];
  98     }
  99 }
 100
 101 extern "C" void TIMER1_IRQHandler (void){
 102     LPC_TIM1->IR |= 1 << 0;
 103     global_step_ticker->reset_tick();
 104 }
 105
 106 // The actual interrupt handler where we do all the work
 107 extern "C" void TIMER0_IRQHandler (void){
 108
 109     LPC_GPIO1->FIOSET = 1<<18;
 110
 111     uint32_t start_time = LPC_TIM0->TC;
 112
 113     LPC_TIM0->IR |= 1 << 0;
 114
 115     global_step_ticker->debug++;
 116
 117     // If no axes enabled, just ignore for now
 118     if( !global_step_ticker->has_axes ){
 119
 120         LPC_GPIO1->FIOCLR = 1<<18;
 121         return;
 122
 123     }
 124
 125     // Do not get out of here before everything is nice and tidy
 126     LPC_TIM0->MR0 = 2000000;
 127
 128     // Step pins
 129     global_step_ticker->tick();
 130
 131     LPC_TIM1->TCR = 3;
 132     LPC_TIM1->TCR = 1;
 133
 134     // If we went over the duration an interrupt is supposed to last, we have a problem
 135     // That can happen tipically when we change blocks, where more than usual computation is done
 136     // This can be OK, if we take notice of it, which we do now
 137     if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO : remove the size condition
 138
 139         uint32_t start_tc = LPC_TIM0->TC;
 140
 141         // 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 )
 142         uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
 143
 144         // Next step is now to reduce this to how many steps we can *actually* skip
 145         uint32_t ticks_we_actually_can_skip = ticks_to_skip;
 146         for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
 147             StepperMotor* stepper = global_step_ticker->stepper_motors[i];
 148             if( stepper->moving ){ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ); }
 149         }
 150
 151         // If the number of ticks we can actually skip is smaller than the number we wanted to skip, there is something wrong in the settings
 152         //if( ticks_we_actually_can_skip < ticks_to_skip ){ }
 153
 154         // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
 155         for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
 156             StepperMotor* stepper = global_step_ticker->stepper_motors[i];
 157             if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
 158         }
 159
 160         // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
 161         LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
 162         //LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
 163
 164         // This is so that we know how long this computation takes, and we can take it into account next time
 165         global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
 166
 167
 168     }else{
 169         LPC_TIM0->MR0 = global_step_ticker->period;
 170     }
 171
 172
 173
 174     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
 175     //    LPC_TIM0->TCR = 3;  // Reset
 176     //    LPC_TIM0->TCR = 1;  // Reset
 177         LPC_TIM0->TC = LPC_TIM0->MR0 - 2;
 178     }
 179
 180
 181     LPC_GPIO1->FIOCLR = 1<<18;
 182
 183 }
 184
 185
 186 // We make a list of steppers that want to be called so that we don't call them for nothing
 187 void StepTicker::add_motor_to_active_list(StepperMotor* motor){
 188     //printf("adding %p with exit: %u \r\n", motor, motor->exit_tick );
 189     uint8_t current_id = 0;
 190     StepperMotor* current = this->active_motors[0];
 191     while(current != NULL ){
 192         if( current == motor ){
 193             return;  // Motor already in list
 194         }
 195         current_id++;
 196         current = this->active_motors[current_id];
 197     }
 198     this->active_motors[current_id  ] = motor;
 199     this->active_motors[current_id+1] = NULL;
 200
 201 }
 202
 203 void StepTicker::remove_motor_from_active_list(StepperMotor* motor){
 204     //printf("removing %p with exit: %u \r\n", motor, motor->exit_tick );
 205     uint8_t current_id = 0;
 206     uint8_t offset = 0;
 207     StepperMotor* current = this->active_motors[0];
 208     while( current != NULL ){
 209         if( current == motor ){ offset++; }
 210         this->active_motors[current_id] = this->active_motors[current_id+offset];
 211         current_id++;
 212         current = this->active_motors[current_id+offset];
 213     }
 214     this->active_motors[current_id] = NULL;
 215
 216
 217 }
 218
 219