X-Git-Url: https://git.hcoop.net/clinton/Smoothieware.git/blobdiff_plain/37102904fd817bf7f9f8743dd361323620852ca9..95bb7f0464d8eabdb7ee315b0e3dd3f67eab60dd:/src/libs/StepTicker.cpp diff --git a/src/libs/StepTicker.cpp b/src/libs/StepTicker.cpp dissimilarity index 68% index 87e0822a..54c4406f 100644 --- a/src/libs/StepTicker.cpp +++ b/src/libs/StepTicker.cpp @@ -1,269 +1,276 @@ -/* - This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). - 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. - 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. - You should have received a copy of the GNU General Public License along with Smoothie. If not, see . -*/ - - -#include "StepTicker.h" - -#include "libs/nuts_bolts.h" -#include "libs/Module.h" -#include "libs/Kernel.h" -#include "StepperMotor.h" -#include "StreamOutputPool.h" -#include "system_LPC17xx.h" // mbed.h lib -#include -#include - -#ifdef STEPTICKER_DEBUG_PIN -#include "gpio.h" -extern GPIO stepticker_debug_pin; -#endif - - -// StepTicker handles the base frequency ticking for the Stepper Motors / Actuators -// It has a list of those, and calls their tick() functions at regular intervals -// They then do Bresenham stuff themselves - -StepTicker* StepTicker::global_step_ticker; - -StepTicker::StepTicker(){ - StepTicker::global_step_ticker = this; - - // Configure the timer - LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register - LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 - LPC_TIM0->TCR = 0; // Disable interrupt - - LPC_SC->PCONP |= (1 << 2); // Power Ticker ON - LPC_TIM1->MR0 = 1000000; - LPC_TIM1->MCR = 1; - LPC_TIM1->TCR = 0; // Disable interrupt - - // Setup RIT timer - LPC_SC->PCONP |= (1L<<16); // RIT Power - LPC_SC->PCLKSEL1 &= ~(3L << 26); // Clear PCLK_RIT bits; - LPC_SC->PCLKSEL1 |= (1L << 26); // Set PCLK_RIT bits to 0x01; - LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * 1000)-1); // 1ms period - LPC_RIT->RICOUNTER = 0; - // Set counter clear/reset after interrupt - LPC_RIT->RICTRL |= (2L); //RITENCLR - LPC_RIT->RICTRL &= ~(8L); // disable - //NVIC_SetVector(RIT_IRQn, (uint32_t)&_ritisr); - - // Default start values - this->a_move_finished = false; - this->do_move_finished = 0; - this->unstep.reset(); - this->set_frequency(100000); - this->set_reset_delay(100); - this->set_acceleration_ticks_per_second(1000); - this->num_motors= 0; - this->active_motor.reset(); - this->tick_cnt= 0; - probe_fnc= nullptr; -} - -StepTicker::~StepTicker() { -} - -//called when everythinf is setup and interrupts can start -void StepTicker::start() { - NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler - NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler - NVIC_EnableIRQ(RIT_IRQn); -} - -// Set the base stepping frequency -void StepTicker::set_frequency( float frequency ){ - this->frequency = frequency; - this->period = floorf((SystemCoreClock/4.0F)/frequency); // SystemCoreClock/4 = Timer increments in a second - LPC_TIM0->MR0 = this->period; - if( LPC_TIM0->TC > LPC_TIM0->MR0 ){ - LPC_TIM0->TCR = 3; // Reset - LPC_TIM0->TCR = 1; // Reset - } -} - -// Set the reset delay -void StepTicker::set_reset_delay( float microseconds ){ - uint32_t delay = floorf((SystemCoreClock/4.0F)*(microseconds/1000000.0F)); // SystemCoreClock/4 = Timer increments in a second - LPC_TIM1->MR0 = delay; -} - -// this is the number of acceleration ticks per second -void StepTicker::set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second) { - uint32_t us= roundf(1000000.0F/acceleration_ticks_per_second); // period in microseconds - LPC_RIT->RICOMPVAL = (uint32_t)(((SystemCoreClock / 1000000L) * us)-1); // us - LPC_RIT->RICOUNTER = 0; - LPC_RIT->RICTRL |= (8L); // Enable rit -} - -// Synchronize the acceleration timer, and optionally schedule it to fire now -void StepTicker::synchronize_acceleration(bool fire_now) { - LPC_RIT->RICOUNTER = 0; - if(fire_now){ - NVIC_SetPendingIRQ(RIT_IRQn); - }else{ - if(NVIC_GetPendingIRQ(RIT_IRQn)) { - // clear pending interrupt so it does not interrupt immediately - LPC_RIT->RICTRL |= 1L; // also clear the interrupt in case it fired - NVIC_ClearPendingIRQ(RIT_IRQn); - } - } -} - - -// Call signal_move_finished() on each active motor that asked to be signaled. We do this instead of inside of tick() so that -// all tick()s are called before we do the move finishing -void StepTicker::signal_a_move_finished(){ - for (int motor = 0; motor < num_motors; motor++){ - if (this->active_motor[motor] && this->motor[motor]->is_move_finished){ - this->motor[motor]->signal_move_finished(); - // Theoretically this does nothing and the reason for it is currently unknown and/or forgotten - // if(this->motor[motor]->moving == false){ - // if (motor > 0){ - // motor--; - // bitmask >>= 1; - // } - // } - } - } -} - -// Reset step pins on any motor that was stepped -inline void StepTicker::unstep_tick(){ - for (int i = 0; i < num_motors; i++) { - if(this->unstep[i]){ - this->motor[i]->unstep(); - } - } - this->unstep.reset(); -} - -extern "C" void TIMER1_IRQHandler (void){ - LPC_TIM1->IR |= 1 << 0; - StepTicker::global_step_ticker->unstep_tick(); -} - -// The actual interrupt handler where we do all the work -extern "C" void TIMER0_IRQHandler (void){ - StepTicker::global_step_ticker->TIMER0_IRQHandler(); -} - -extern "C" void RIT_IRQHandler (void){ - LPC_RIT->RICTRL |= 1L; - StepTicker::global_step_ticker->acceleration_tick(); -} - -extern "C" void PendSV_Handler(void) { - StepTicker::global_step_ticker->PendSV_IRQHandler(); -} - -// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking -void StepTicker::PendSV_IRQHandler (void) { - - if(this->do_move_finished.load() > 0) { - this->do_move_finished--; - #ifdef STEPTICKER_DEBUG_PIN - stepticker_debug_pin= 1; - #endif - - this->signal_a_move_finished(); - - #ifdef STEPTICKER_DEBUG_PIN - stepticker_debug_pin= 0; - #endif - } -} - -// run in RIT lower priority than PendSV -void StepTicker::acceleration_tick() { - // call registered acceleration handlers - for (size_t i = 0; i < acceleration_tick_handlers.size(); ++i) { - acceleration_tick_handlers[i](); - } -} - -void StepTicker::TIMER0_IRQHandler (void){ - // Reset interrupt register - LPC_TIM0->IR |= 1 << 0; - tick_cnt++; // count number of ticks - - // check if we are probing and if so check the probe status and force an end of move if it is triggered - if(probe_fnc && probe_fnc()) { - // we do exactly what we would do if stepping had finished - for (uint32_t motor = 0; motor < num_motors; motor++){ - if(this->active_motor[motor]){ - this->motor[motor]->force_finish_move(); - } - } - - }else{ - - // 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 - for (uint32_t motor = 0; motor < num_motors; motor++){ - // send tick to all active motors - if(this->active_motor[motor] && this->motor[motor]->tick()){ - // we stepped so schedule an unstep - this->unstep[motor]= 1; - } - } - } - - // We may have set a pin on in this tick, now we reset the timer to set it off - // Note there could be a race here if we run another tick before the unsteps have happened, - // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue - // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us - if( this->unstep.any()){ - LPC_TIM1->TCR = 3; - LPC_TIM1->TCR = 1; - } - // just let it run it will fire every 143 seconds - // else{ - // LPC_TIM1->TCR = 0; // disable interrupt, no point in it running if nothing to do - // } - - if(this->a_move_finished) { - this->a_move_finished= false; - 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 - } - - // If a move finished in this tick, we have to tell the actuator to act accordingly - if(this->do_move_finished.load() > 0){ - // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits - //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work - SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk; - } -} - -// returns index of the stepper motor in the array and bitset -int StepTicker::register_motor(StepperMotor* motor) -{ - this->motor.push_back(motor); - this->num_motors= this->motor.size(); - return this->num_motors-1; -} - -// activate the specified motor, must have been registered -void StepTicker::add_motor_to_active_list(StepperMotor* motor) -{ - bool enabled= active_motor.any(); // see if interrupt was previously enabled - active_motor[motor->index]= 1; - if(!enabled) { - LPC_TIM0->TCR = 1; // Enable interrupt - } -} - -// Remove a stepper from the list of active motors -void StepTicker::remove_motor_from_active_list(StepperMotor* motor) -{ - active_motor[motor->index]= 0; - // If we have no motor to work on, disable the whole interrupt - if(this->active_motor.none()){ - LPC_TIM0->TCR = 0; // Disable interrupt - tick_cnt= 0; - } -} +/* + This file is part of Smoothie (http://smoothieware.org/). The motion control part is heavily based on Grbl (https://github.com/simen/grbl). + 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. + 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. + You should have received a copy of the GNU General Public License along with Smoothie. If not, see . +*/ + + +#include "StepTicker.h" + +#include "libs/nuts_bolts.h" +#include "libs/Module.h" +#include "libs/Kernel.h" +#include "StepperMotor.h" +#include "StreamOutputPool.h" +#include "Block.h" +#include "Conveyor.h" + +#include "system_LPC17xx.h" // mbed.h lib +#include +#include + +#ifdef STEPTICKER_DEBUG_PIN +// debug pins, only used if defined in src/makefile +#include "gpio.h" +GPIO stepticker_debug_pin(STEPTICKER_DEBUG_PIN); +#define SET_STEPTICKER_DEBUG_PIN(n) {if(n) stepticker_debug_pin.set(); else stepticker_debug_pin.clear(); } +#else +#define SET_STEPTICKER_DEBUG_PIN(n) +#endif + +StepTicker *StepTicker::instance; + +StepTicker::StepTicker() +{ + instance = this; // setup the Singleton instance of the stepticker + + // Configure the timer + LPC_TIM0->MR0 = 10000000; // Initial dummy value for Match Register + LPC_TIM0->MCR = 3; // Match on MR0, reset on MR0, match on MR1 + LPC_TIM0->TCR = 0; // Disable interrupt + + LPC_SC->PCONP |= (1 << 2); // Power Ticker ON + LPC_TIM1->MR0 = 1000000; + LPC_TIM1->MCR = 1; + LPC_TIM1->TCR = 0; // Disable interrupt + + // Default start values + this->set_frequency(100000); + this->set_unstep_time(100); + + this->unstep.reset(); + this->num_motors = 0; + + this->running = false; + this->current_block = nullptr; + + #ifdef STEPTICKER_DEBUG_PIN + // setup debug pin if defined + stepticker_debug_pin.output(); + stepticker_debug_pin= 0; + #endif +} + +StepTicker::~StepTicker() +{ +} + +//called when everything is setup and interrupts can start +void StepTicker::start() +{ + NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler + NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler +} + +// Set the base stepping frequency +void StepTicker::set_frequency( float frequency ) +{ + this->frequency = frequency; + this->period = floorf((SystemCoreClock / 4.0F) / frequency); // SystemCoreClock/4 = Timer increments in a second + LPC_TIM0->MR0 = this->period; + LPC_TIM0->TCR = 3; // Reset + LPC_TIM0->TCR = 1; // start +} + +// Set the reset delay +void StepTicker::set_unstep_time( float microseconds ) +{ + uint32_t delay = floorf((SystemCoreClock / 4.0F) * (microseconds / 1000000.0F)); // SystemCoreClock/4 = Timer increments in a second + LPC_TIM1->MR0 = delay; +} + +// Reset step pins on any motor that was stepped +void StepTicker::unstep_tick() +{ + for (int i = 0; i < num_motors; i++) { + if(this->unstep[i]) { + this->motor[i]->unstep(); + } + } + this->unstep.reset(); +} + +extern "C" void TIMER1_IRQHandler (void) +{ + LPC_TIM1->IR |= 1 << 0; + StepTicker::getInstance()->unstep_tick(); +} + +// The actual interrupt handler where we do all the work +extern "C" void TIMER0_IRQHandler (void) +{ + // Reset interrupt register + LPC_TIM0->IR |= 1 << 0; + StepTicker::getInstance()->step_tick(); +} + +extern "C" void PendSV_Handler(void) +{ + StepTicker::getInstance()->handle_finish(); +} + +// slightly lower priority than TIMER0, the whole end of block/start of block is done here allowing the timer to continue ticking +void StepTicker::handle_finish (void) +{ + // all moves finished signal block is finished + if(finished_fnc) finished_fnc(); +} + +// step clock +void StepTicker::step_tick (void) +{ + static uint32_t current_tick = 0; + + //SET_STEPTICKER_DEBUG_PIN(running ? 1 : 0); + + // if nothing has been setup we ignore the ticks + if(!running){ + // check if anything new available + if(THECONVEYOR->get_next_block(¤t_block)) { // returns false if no new block is available + running= start_next_block(); // returns true if there is at least one motor with steps to issue + if(!running) return; + }else{ + return; + } + } + + if(THEKERNEL->is_halted()) { + running= false; + return; + } + + bool still_moving= false; + // foreach motor, if it is active see if time to issue a step to that motor + for (uint8_t m = 0; m < num_motors; m++) { + if(current_block->tick_info[m].steps_to_move == 0) continue; // not active + + current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change; + + if(current_tick == current_block->tick_info[m].next_accel_event) { + if(current_tick == current_block->accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau + current_block->tick_info[m].acceleration_change = 0; + if(current_block->decelerate_after < current_block->total_move_ticks) { + current_block->tick_info[m].next_accel_event = current_block->decelerate_after; + if(current_tick != current_block->decelerate_after) { // We are plateauing + // steps/sec / tick frequency to get steps per tick + current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate; + } + } + } + + if(current_tick == current_block->decelerate_after) { // We start decelerating + current_block->tick_info[m].acceleration_change = current_block->tick_info[m].deceleration_change; + } + } + + // protect against rounding errors and such + if(current_block->tick_info[m].steps_per_tick <= 0) { + current_block->tick_info[m].counter = STEPTICKER_FPSCALE; // we force completion this step by setting to 1.0 + current_block->tick_info[m].steps_per_tick = 0; + } + + current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick; + + if(current_block->tick_info[m].counter >= STEPTICKER_FPSCALE) { // >= 1.0 step time + current_block->tick_info[m].counter -= STEPTICKER_FPSCALE; // -= 1.0F; + ++current_block->tick_info[m].step_count; + + // step the motor + bool ismoving= motor[m]->step(); // returns false if the moving flag was set to false externally (probes, endstops etc) + // we stepped so schedule an unstep + unstep.set(m); + + if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) { + // done + current_block->tick_info[m].steps_to_move = 0; + motor[m]->moving= false; // let motor know it is no longer moving + } + } + + // see if any motors are still moving after this tick + if(motor[m]->moving) still_moving= true; + } + + // do this after so we start at tick 0 + current_tick++; // count number of ticks + + // We may have set a pin on in this tick, now we reset the timer to set it off + // Note there could be a race here if we run another tick before the unsteps have happened, + // right now it takes about 3-4us but if the unstep were near 10uS or greater it would be an issue + // also it takes at least 2us to get here so even when set to 1us pulse width it will still be about 3us + if( unstep.any()) { + LPC_TIM1->TCR = 3; + LPC_TIM1->TCR = 1; + } + + + // see if any motors are still moving + if(!still_moving) { + SET_STEPTICKER_DEBUG_PIN(0); + + // all moves finished + current_tick = 0; + + // get next block + // do it here so there is no delay in ticks + THECONVEYOR->block_finished(); + + if(THECONVEYOR->get_next_block(¤t_block)) { // returns false if no new block is available + running= start_next_block(); // returns true if there is at least one motor with steps to issue + + }else{ + current_block= nullptr; + running= false; + } + + // all moves finished + // we delegate the slow stuff to the pendsv handler which will run as soon as this interrupt exits + //NVIC_SetPendingIRQ(PendSV_IRQn); this doesn't work + //SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk; + } +} + +// only called from the step tick ISR (single consumer) +bool StepTicker::start_next_block() +{ + if(current_block == nullptr) return false; + + bool ok= false; + // need to prepare each active motor + for (uint8_t m = 0; m < num_motors; m++) { + if(current_block->tick_info[m].steps_to_move == 0) continue; + + ok= true; // mark at least one motor is moving + // set direction bit here + // NOTE this would be at least 10us before first step pulse. + // TODO does this need to be done sooner, if so how without delaying next tick + motor[m]->set_direction(current_block->direction_bits[m]); + motor[m]->moving= true; // also let motor know it is moving now + } + + if(ok) { + SET_STEPTICKER_DEBUG_PIN(1); + return true; + } + + return false; +} + + +// returns index of the stepper motor in the array and bitset +int StepTicker::register_motor(StepperMotor* m) +{ + motor[num_motors++] = m; + return num_motors - 1; +}