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 #include "libs/nuts_bolts.h"
  12 #include "libs/Module.h"
  13 #include "libs/Kernel.h"
  14 #include "StepperMotor.h"
  15 #include "StreamOutputPool.h"
  16 #include "system_LPC17xx.h" // mbed.h lib
  17 #include <math.h>
  18 #include <mri.h>
  19
  20 #ifdef STEPTICKER_DEBUG_PIN
  21 #include "gpio.h"
  22 extern GPIO stepticker_debug_pin;
  23 #endif
  24
  25
  26 // StepTicker handles the base frequency ticking for the Stepper Motors / Actuators
  27 // It has a list of those, and calls their tick() functions at regular intervals
  28 // They then do Bresenham stuff themselves
  29
  30 StepTicker* StepTicker::global_step_ticker;
  31
  32 StepTicker::StepTicker(){
  33     StepTicker::global_step_ticker = this;
  34
  35     // Configure the timer
  36     LPC_TIM0->MR0 = 10000000;       // Initial dummy value for Match Register
  37     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
  38     LPC_TIM0->TCR = 0;              // Disable interrupt
  39
  40     LPC_SC->PCONP |= (1 << 2);      // Power Ticker ON
  41     LPC_TIM1->MR0 = 1000000;
  42     LPC_TIM1->MCR = 1;
  43     LPC_TIM1->TCR = 0;              // Disable interrupt
  44
  45     // Setup RIT timer
  46     LPC_SC->PCONP |= (1L<<16); // RIT Power
  47     LPC_SC->PCLKSEL1 &= ~(3L << 26); // Clear PCLK_RIT bits;
  48     LPC_SC->PCLKSEL1 |=  (1L << 26); // Set PCLK_RIT bits to 0x01;
  49     LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * 1000)-1); // 1ms period
  50     LPC_RIT->RICOUNTER = 0;
  51     // Set counter clear/reset after interrupt
  52     LPC_RIT->RICTRL |= (2L); //RITENCLR
  53     LPC_RIT->RICTRL &= ~(8L); // disable
  54     //NVIC_SetVector(RIT_IRQn, (uint32_t)&_ritisr);
  55
  56     // Default start values
  57     this->a_move_finished = false;
  58     this->do_move_finished = 0;
  59     this->unstep.reset();
  60     this->set_frequency(100000);
  61     this->set_reset_delay(100);
  62     this->set_acceleration_ticks_per_second(1000);
  63     this->num_motors= 0;
  64     this->active_motor.reset();
  65     this->tick_cnt= 0;
  66 }
  67
  68 StepTicker::~StepTicker() {
  69 }
  70
  71 //called when everythinf is setup and interrupts can start
  72 void StepTicker::start() {
  73     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  74     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  75     NVIC_EnableIRQ(RIT_IRQn);
  76 }
  77
  78 // Set the base stepping frequency
  79 void StepTicker::set_frequency( float frequency ){
  80     this->frequency = frequency;
  81     this->period = floorf((SystemCoreClock/4.0F)/frequency);  // SystemCoreClock/4 = Timer increments in a second
  82     LPC_TIM0->MR0 = this->period;
  83     if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
  84         LPC_TIM0->TCR = 3;  // Reset
  85         LPC_TIM0->TCR = 1;  // Reset
  86     }
  87 }
  88
  89 // Set the reset delay
  90 void StepTicker::set_reset_delay( float microseconds ){
  91     uint32_t delay = floorf((SystemCoreClock/4.0F)*(microseconds/1000000.0F));  // SystemCoreClock/4 = Timer increments in a second
  92     LPC_TIM1->MR0 = delay;
  93 }
  94
  95 // this is the number of acceleration ticks per second
  96 void StepTicker::set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second) {
  97     uint32_t us= roundf(1000000.0F/acceleration_ticks_per_second); // period in microseconds
  98     LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * us)-1); // us
  99     LPC_RIT->RICOUNTER = 0;
 100     LPC_RIT->RICTRL |= (8L); // Enable rit
 101 }
 102
 103 // Synchronize the acceleration timer, and optionally schedule it to fire now
 104 void StepTicker::synchronize_acceleration(bool fire_now) {
 105     LPC_RIT->RICOUNTER = 0;
 106     if(fire_now){
 107         NVIC_SetPendingIRQ(RIT_IRQn);
 108     }else{
 109         if(NVIC_GetPendingIRQ(RIT_IRQn)) {
 110             // clear pending interrupt so it does not interrupt immediately
 111             LPC_RIT->RICTRL |= 1L; // also clear the interrupt in case it fired
 112             NVIC_ClearPendingIRQ(RIT_IRQn);
 113         }
 114     }
 115 }
 116
 117
 118 // Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that
 119 // all tick()s are called before we do the move finishing
 120 void StepTicker::signal_a_move_finished(){
 121      for (int motor = 0; motor < num_motors; motor++){
 122         if (this->active_motor[motor] && this->motor[motor]->is_move_finished){
 123             this->motor[motor]->signal_move_finished();
 124                 // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten
 125                 // if(this->motor[motor]->moving == false){
 126                 //     if (motor > 0){
 127                 //         motor--;
 128                 //         bitmask >>= 1;
 129                 //     }
 130                 // }
 131         }
 132     }
 133 }
 134
 135 // Reset step pins on any motor that was stepped
 136 inline void StepTicker::unstep_tick(){
 137     for (int i = 0; i < num_motors; i++) {
 138         if(this->unstep[i]){
 139             this->motor[i]->unstep();
 140         }
 141     }
 142     this->unstep.reset();
 143 }
 144
 145 extern "C" void TIMER1_IRQHandler (void){
 146     LPC_TIM1->IR |= 1 << 0;
 147     StepTicker::global_step_ticker->unstep_tick();
 148 }
 149
 150 // The actual interrupt handler where we do all the work
 151 extern "C" void TIMER0_IRQHandler (void){
 152     StepTicker::global_step_ticker->TIMER0_IRQHandler();
 153 }
 154
 155 extern "C" void RIT_IRQHandler (void){
 156     LPC_RIT->RICTRL |= 1L;
 157     StepTicker::global_step_ticker->acceleration_tick();
 158 }
 159
 160 extern "C" void PendSV_Handler(void) {
 161     StepTicker::global_step_ticker->PendSV_IRQHandler();
 162 }
 163
 164 // slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking
 165 void StepTicker::PendSV_IRQHandler (void) {
 166
 167     if(this->do_move_finished.load() > 0) {
 168         this->do_move_finished--;
 169         #ifdef STEPTICKER_DEBUG_PIN
 170         stepticker_debug_pin= 1;
 171         #endif
 172
 173         this->signal_a_move_finished();
 174
 175         #ifdef STEPTICKER_DEBUG_PIN
 176         stepticker_debug_pin= 0;
 177         #endif
 178     }
 179 }
 180
 181 // run in RIT lower priority than PendSV
 182 void  StepTicker::acceleration_tick() {
 183     // call registered acceleration handlers
 184     for (size_t i = 0; i < acceleration_tick_handlers.size(); ++i) {
 185         acceleration_tick_handlers[i]();
 186     }
 187 }
 188
 189 void StepTicker::TIMER0_IRQHandler (void){
 190     // Reset interrupt register
 191     LPC_TIM0->IR |= 1 << 0;
 192     tick_cnt++; // count number of ticks
 193
 194     // Step pins NOTE takes 1.2us when nothing to step, 1.8-2us for one motor stepped and 2.6us when two motors stepped, 3.167us when three motors stepped
 195     for (uint32_t motor = 0; motor < num_motors; motor++){
 196         // send tick to all active motors
 197         if(this->active_motor[motor] && this->motor[motor]->tick()){
 198             // we stepped so schedule an unstep
 199             this->unstep[motor]= 1;
 200         }
 201     }
 202
 203     // We may have set a pin on in this tick, now we reset the timer to set it off
 204     // Note there could be a race here if we run another tick before the unsteps have happened,
 205     // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue
 206     // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us
 207     if( this->unstep.any()){
 208         LPC_TIM1->TCR = 3;
 209         LPC_TIM1->TCR = 1;
 210     }
 211     // just let it run it will fire every 143 seconds
 212     // else{
 213     //     LPC_TIM1->TCR = 0; // disable interrupt, no point in it running if nothing to do
 214     // }
 215
 216     if(this->a_move_finished) {
 217         this->a_move_finished= false;
 218         this->do_move_finished++; // Note this is an atomic variable because it is updated in two interrupts of different priorities so can be pre-empted
 219     }
 220
 221     // If a move finished in this tick, we have to tell the actuator to act accordingly
 222     if(this->do_move_finished.load() > 0){
 223         // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits
 224         //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work
 225         SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
 226     }
 227 }
 228
 229 // returns index of the stepper motor in the array and bitset
 230 int StepTicker::register_motor(StepperMotor* motor)
 231 {
 232     this->motor.push_back(motor);
 233     this->num_motors= this->motor.size();
 234     return this->num_motors-1;
 235 }
 236
 237 // activate the specified motor, must have been registered
 238 void StepTicker::add_motor_to_active_list(StepperMotor* motor)
 239 {
 240     bool enabled= active_motor.any(); // see if interrupt was previously enabled
 241     active_motor[motor->index]= 1;
 242     if(!enabled) {
 243         LPC_TIM0->TCR = 1;               // Enable interrupt
 244     }
 245 }
 246
 247 // Remove a stepper from the list of active motors
 248 void StepTicker::remove_motor_from_active_list(StepperMotor* motor)
 249 {
 250     active_motor[motor->index]= 0;
 251     // If we have no motor to work on, disable the whole interrupt
 252     if(this->active_motor.none()){
 253         LPC_TIM0->TCR = 0;               // Disable interrupt
 254         tick_cnt= 0;
 255     }
 256 }