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