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 "Block.h"
  17 #include "Conveyor.h"
  18
  19 #include "system_LPC17xx.h" // mbed.h lib
  20 #include <math.h>
  21 #include <mri.h>
  22
  23 #ifdef STEPTICKER_DEBUG_PIN
  24 // debug pins, only used if defined in src/makefile
  25 #include "gpio.h"
  26 GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN);
  27 #define SET_STEPTICKER_DEBUG_PIN(n) {if(n) stepticker_debug_pin.set(); else stepticker_debug_pin.clear(); }
  28 #else
  29 #define SET_STEPTICKER_DEBUG_PIN(n)
  30 #endif
  31
  32 StepTicker *StepTicker::instance;
  33
  34 StepTicker::StepTicker()
  35 {
  36     instance = this; // setup the Singleton instance of the stepticker
  37
  38     // Configure the timer
  39     LPC_TIM0->MR0 = 10000000;       // Initial dummy value for Match Register
  40     LPC_TIM0->MCR = 3;              // Match on MR0, reset on MR0, match on MR1
  41     LPC_TIM0->TCR = 0;              // Disable interrupt
  42
  43     LPC_SC->PCONP |= (1 << 2);      // Power Ticker ON
  44     LPC_TIM1->MR0 = 1000000;
  45     LPC_TIM1->MCR = 1;
  46     LPC_TIM1->TCR = 0;              // Disable interrupt
  47
  48     // Default start values
  49     this->set_frequency(100000);
  50     this->set_unstep_time(100);
  51
  52     this->unstep.reset();
  53     this->num_motors = 0;
  54
  55     this->running = false;
  56     this->current_block = nullptr;
  57
  58     #ifdef STEPTICKER_DEBUG_PIN
  59     // setup debug pin if defined
  60     stepticker_debug_pin.output();
  61     stepticker_debug_pin= 0;
  62     #endif
  63 }
  64
  65 StepTicker::~StepTicker()
  66 {
  67 }
  68
  69 //called when everything is setup and interrupts can start
  70 void StepTicker::start()
  71 {
  72     NVIC_EnableIRQ(TIMER0_IRQn);     // Enable interrupt handler
  73     NVIC_EnableIRQ(TIMER1_IRQn);     // Enable interrupt handler
  74 }
  75
  76 // Set the base stepping frequency
  77 void StepTicker::set_frequency( float frequency )
  78 {
  79     this->frequency = frequency;
  80     this->period = floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second
  81     LPC_TIM0->MR0 = this->period;
  82     LPC_TIM0->TCR = 3;  // Reset
  83     LPC_TIM0->TCR = 1;  // start
  84 }
  85
  86 // Set the reset delay
  87 void StepTicker::set_unstep_time( float microseconds )
  88 {
  89     uint32_t delay = floorf((SystemCoreClock / 4.0F) * (microseconds / 1000000.0F)); // SystemCoreClock/4 = Timer increments in a second
  90     LPC_TIM1->MR0 = delay;
  91 }
  92
  93 // Reset step pins on any motor that was stepped
  94 void StepTicker::unstep_tick()
  95 {
  96     for (int i = 0; i < num_motors; i++) {
  97         if(this->unstep[i]) {
  98             this->motor[i]->unstep();
  99         }
 100     }
 101     this->unstep.reset();
 102 }
 103
 104 extern "C" void TIMER1_IRQHandler (void)
 105 {
 106     LPC_TIM1->IR |= 1 << 0;
 107     StepTicker::getInstance()->unstep_tick();
 108 }
 109
 110 // The actual interrupt handler where we do all the work
 111 extern "C" void TIMER0_IRQHandler (void)
 112 {
 113     // Reset interrupt register
 114     LPC_TIM0->IR |= 1 << 0;
 115     StepTicker::getInstance()->step_tick();
 116 }
 117
 118 extern "C" void PendSV_Handler(void)
 119 {
 120     StepTicker::getInstance()->handle_finish();
 121 }
 122
 123 // slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking
 124 void StepTicker::handle_finish (void)
 125 {
 126     // all moves finished signal block is finished
 127     if(finished_fnc) finished_fnc();
 128 }
 129
 130 // step clock
 131 void StepTicker::step_tick (void)
 132 {
 133     static uint32_t current_tick = 0;
 134
 135     //SET_STEPTICKER_DEBUG_PIN(running ? 1 : 0);
 136
 137     // if nothing has been setup we ignore the ticks
 138     if(!running){
 139         // check if anything new available
 140         if(THECONVEYOR->get_next_block(&current_block)) { // returns false if no new block is available
 141             running= start_next_block(); // returns true if there is at least one motor with steps to issue
 142             if(!running) return;
 143         }else{
 144             return;
 145         }
 146     }
 147
 148     if(THEKERNEL->is_halted()) {
 149         running= false;
 150         return;
 151     }
 152
 153     bool still_moving= false;
 154     // foreach motor, if it is active see if time to issue a step to that motor
 155     for (uint8_t m = 0; m < num_motors; m++) {
 156         if(current_block->tick_info[m].steps_to_move == 0) continue; // not active
 157
 158         current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change;
 159
 160         if(current_tick == current_block->tick_info[m].next_accel_event) {
 161             if(current_tick == current_block->accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau
 162                 current_block->tick_info[m].acceleration_change = 0;
 163                 if(current_block->decelerate_after < current_block->total_move_ticks) {
 164                     current_block->tick_info[m].next_accel_event = current_block->decelerate_after;
 165                     if(current_tick != current_block->decelerate_after) { // We are plateauing
 166                         // steps/sec / tick frequency to get steps per tick
 167                         current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate;
 168                     }
 169                 }
 170             }
 171
 172             if(current_tick == current_block->decelerate_after) { // We start decelerating
 173                 current_block->tick_info[m].acceleration_change = current_block->tick_info[m].deceleration_change;
 174             }
 175         }
 176
 177         // protect against rounding errors and such
 178         if(current_block->tick_info[m].steps_per_tick <= 0) {
 179             current_block->tick_info[m].counter = STEPTICKER_FPSCALE; // we force completion this step by setting to 1.0
 180             current_block->tick_info[m].steps_per_tick = 0;
 181         }
 182
 183         current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick;
 184
 185         if(current_block->tick_info[m].counter >= STEPTICKER_FPSCALE) { // >= 1.0 step time
 186             current_block->tick_info[m].counter -= STEPTICKER_FPSCALE; // -= 1.0F;
 187             ++current_block->tick_info[m].step_count;
 188
 189             // step the motor
 190             motor[m]->step();
 191             // we stepped so schedule an unstep
 192             unstep.set(m);
 193
 194             if(current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) {
 195                 // done
 196                 current_block->tick_info[m].steps_to_move = 0;
 197                 motor[m]->moving= false; // let motor know it is no longer moving
 198             }
 199         }
 200
 201         // see if any motors are still moving after this tick
 202         if(motor[m]->moving) still_moving= true;
 203     }
 204
 205     // do this after so we start at tick 0
 206     current_tick++; // count number of ticks
 207
 208     // We may have set a pin on in this tick, now we reset the timer to set it off
 209     // Note there could be a race here if we run another tick before the unsteps have happened,
 210     // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue
 211     // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us
 212     if( unstep.any()) {
 213         LPC_TIM1->TCR = 3;
 214         LPC_TIM1->TCR = 1;
 215     }
 216
 217
 218     // see if any motors are still moving
 219     if(!still_moving) {
 220         SET_STEPTICKER_DEBUG_PIN(0);
 221
 222         // all moves finished
 223         current_tick = 0;
 224
 225         // get next block
 226         // do it here so there is no delay in ticks
 227         THECONVEYOR->block_finished();
 228
 229         if(THECONVEYOR->get_next_block(&current_block)) { // returns false if no new block is available
 230             running= start_next_block(); // returns true if there is at least one motor with steps to issue
 231
 232         }else{
 233             current_block= nullptr;
 234             running= false;
 235         }
 236
 237         // all moves finished
 238         // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits
 239         //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work
 240         //SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
 241     }
 242 }
 243
 244 // only called from the step tick ISR (single consumer)
 245 bool StepTicker::start_next_block()
 246 {
 247     if(current_block == nullptr) return false;
 248
 249     bool ok= false;
 250     // need to prepare each active motor
 251     for (uint8_t m = 0; m < num_motors; m++) {
 252         if(current_block->tick_info[m].steps_to_move == 0) continue;
 253
 254         ok= true; // mark at least one motor is moving
 255         // set direction bit here
 256         // NOTE this would be at least 10us before first step pulse.
 257         // TODO does this need to be done sooner, if so how without delaying next tick
 258         motor[m]->set_direction(current_block->direction_bits[m]);
 259         motor[m]->moving= true; // also let motor know it is moving now
 260     }
 261
 262     if(ok) {
 263         SET_STEPTICKER_DEBUG_PIN(1);
 264         return true;
 265     }
 266
 267     return false;
 268 }
 269
 270
 271 // returns index of the stepper motor in the array and bitset
 272 int StepTicker::register_motor(StepperMotor* m)
 273 {
 274     motor[num_motors++] = m;
 275     return num_motors - 1;
 276 }