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
  19 StepTicker* global_step_ticker;
  20
  21 StepTicker::StepTicker(){
  22     global_step_ticker = this;
  23     LPC_TIM0->MR0 = 10000000;        // Initial dummy value for Match Register
  24     LPC_TIM0->MR1 = 10000000;
  25     LPC_TIM0->MCR = 11;              // Match on MR0, reset on MR0, match on MR1
  26     LPC_TIM0->TCR = 1;              // Enable interrupt
  27     this->debug = 0;
  28     this->has_axes = 0;
  29     this->set_frequency(0.001);
  30     this->set_reset_delay(100);
  31     this->last_duration = 0;
  32     NVIC_EnableIRQ(TIMER0_IRQn);    // Enable interrupt handler
  33 }
  34
  35 void StepTicker::set_frequency( double frequency ){
  36     this->frequency = frequency;
  37     this->period = int(floor((SystemCoreClock/4)/frequency));  // SystemCoreClock/4 = Timer increments in a second
  38     LPC_TIM0->MR0 = this->period;
  39     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  40         LPC_TIM0->TCR = 3;  // Reset
  41         LPC_TIM0->TCR = 1;  // Reset
  42     }
  43 }
  44
  45 void StepTicker::set_reset_delay( double seconds ){
  46     this->delay = int(floor(double(SystemCoreClock/4)*( seconds )));  // SystemCoreClock/4 = Timer increments in a second
  47 }
  48
  49 // Add a stepper motor object to our list of steppers we must take care of
  50 StepperMotor* StepTicker::add_stepper_motor(StepperMotor* stepper_motor){
  51     this->stepper_motors.push_back(stepper_motor);
  52     stepper_motor->step_ticker = this;
  53     this->has_axes = true;
  54     return stepper_motor;
  55 }
  56
  57 inline void StepTicker::tick(){
  58     for(unsigned int i=0; i < this->stepper_motors.size(); i++){
  59         this->stepper_motors[i]->tick();
  60     }
  61 }
  62
  63 inline void StepTicker::reset_tick(){
  64     for(unsigned int i=0; i < this->stepper_motors.size(); i++){
  65         this->stepper_motors[i]->step_pin->set(0);
  66     }
  67 }
  68
  69 extern "C" void TIMER0_IRQHandler (void){
  70     uint32_t start_time = LPC_TIM0->TC;
  71     global_step_ticker->debug++;
  72
  73     if( !global_step_ticker->has_axes ){
  74         if((LPC_TIM0->IR >> 0) & 1){ LPC_TIM0->IR |= 1 << 0; }
  75         if((LPC_TIM0->IR >> 1) & 1){ LPC_TIM0->IR |= 1 << 1; }
  76         return;
  77     }
  78
  79     LPC_TIM0->MR1 = 2000000;
  80
  81     if((LPC_TIM0->IR >> 0) & 1){  // If interrupt register set for MR0
  82         // Do not get out of here before everything is nice and tidy
  83
  84         //if( global_step_ticker->debug % 1000 == 1 ){ printf("start: tc: %u mr0: %u mr1: %u d: %u\r\n", LPC_TIM0->TC, LPC_TIM0->MR0, LPC_TIM0->MR1, global_step_ticker->debug); }
  85
  86         LPC_TIM0->MR0 = 2000000;
  87
  88         LPC_TIM0->IR |= 1 << 0;   // Reset it
  89
  90         // Step pins
  91         uint32_t aa = LPC_TIM0->TC;
  92         global_step_ticker->tick();
  93         uint32_t bb = LPC_TIM0->TC;
  94
  95         // Maybe we have spent enough time in this interrupt so that we have to reset the pins ourself
  96         if( LPC_TIM0->TC > global_step_ticker->delay ){
  97             global_step_ticker->reset_tick();
  98         }else{
  99             // Else we have to trigger this a tad later, using MR1
 100             LPC_TIM0->MR1 = global_step_ticker->delay;
 101         }
 102         uint32_t cc = LPC_TIM0->TC;
 103
 104         // If we went over the duration an interrupt is supposed to last, we have a problem
 105         // That can happen tipically when we change blocks, where more than usual computation is done
 106         // This can be OK, if we take notice of it, which we do now
 107         if( LPC_TIM0->TC > global_step_ticker->period ){ // TODO : remove the size condition
 108
 109             uint32_t start_tc = LPC_TIM0->TC;
 110
 111             // 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 )
 112             uint32_t ticks_to_skip = (  ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
 113
 114             // Next step is now to reduce this to how many steps we can *actually* skip
 115             uint32_t ticks_we_actually_can_skip = ticks_to_skip;
 116             for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
 117                 StepperMotor* stepper = global_step_ticker->stepper_motors[i];
 118                 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) ); }
 119             }
 120
 121             // 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
 122             if( ticks_we_actually_can_skip < ticks_to_skip ){ }
 123
 124             // Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
 125             for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
 126                 StepperMotor* stepper = global_step_ticker->stepper_motors[i];
 127                 if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
 128             }
 129
 130             // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
 131             LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
 132
 133             // This is so that we know how long this computation takes, and we can take it into account next time
 134             global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
 135
 136             if( global_step_ticker->debug % 50 == 1 || 0 ){
 137                 printf("a\r\n");
 138
 139                 printf("tc:%5u ld:%5u prd:%5u tts:%5u twcas:%5u nmr0:%5u \r\n", start_tc, global_step_ticker->last_duration, global_step_ticker->period, ticks_to_skip, ticks_we_actually_can_skip,  LPC_TIM0->MR0 );
 140                 for(unsigned int j=0; j < global_step_ticker->stepper_motors.size(); j++){
 141                     StepperMotor* stepper = global_step_ticker->stepper_motors[j];
 142                     if( stepper->moving ){
 143                         uint32_t dd = (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ;
 144                         printf("    %u: %u\r\n", j,  dd);
 145                     }
 146                 }
 147
 148
 149             }
 150
 151
 152         }else{
 153             LPC_TIM0->MR0 = global_step_ticker->period;
 154         }
 155
 156     }else{
 157         // Else obviously it's MR1
 158         LPC_TIM0->IR |= 1 << 1;   // Reset it
 159         // Reset pins
 160         global_step_ticker->reset_tick();
 161     }
 162
 163     //printf("end: tc: %u mr0: %u mr1: %u d: %u\r\n", LPC_TIM0->TC, LPC_TIM0->MR0, LPC_TIM0->MR1, global_step_ticker->debug);
 164
 165     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
 166         LPC_TIM0->TCR = 3;  // Reset
 167         LPC_TIM0->TCR = 1;  // Reset
 168     }
 169
 170
 171 }
 172
 173