#include "StepperMotor.h"
#include "StreamOutputPool.h"
#include "Block.h"
+#include "Conveyor.h"
#include "system_LPC17xx.h" // mbed.h lib
#include <math.h>
#include <mri.h>
#ifdef STEPTICKER_DEBUG_PIN
+// debug pins, only used if defined in src/makefile
#include "gpio.h"
-extern GPIO stepticker_debug_pin;
+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;
-// handle 2.30 Fixed point
-#define FPSCALE (1<<30)
-#define TOFP(x) ((int32_t)roundf((float)(x)*FPSCALE))
-#define FROMFP(x) ((float)(x)/FPSCALE)
-
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->MCR = 3; // Match on MR0, reset on MR0
LPC_TIM0->TCR = 0; // Disable interrupt
LPC_SC->PCONP |= (1 << 2); // Power Ticker ON
LPC_TIM1->MR0 = 1000000;
- LPC_TIM1->MCR = 1;
+ LPC_TIM1->MCR = 5; // match on Mr0, stop on match
LPC_TIM1->TCR = 0; // Disable interrupt
// Default start values
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()
{
NVIC_EnableIRQ(TIMER0_IRQn); // Enable interrupt handler
NVIC_EnableIRQ(TIMER1_IRQn); // Enable interrupt handler
+ current_tick= 0;
}
// Set the base stepping frequency
LPC_TIM0->TCR = 1; // start
}
-// Set the reset delay
+// Set the reset delay, must be called after set_frequency
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;
+
+ // TODO check that the unstep time is less than the step period, if not slow down step ticker
}
// Reset step pins on any motor that was stepped
if(finished_fnc) finished_fnc();
}
-
// step clock
void StepTicker::step_tick (void)
{
- static uint32_t current_tick = 0;
-
- //stepticker_debug_pin= running ? 1 : 0;
+ //SET_STEPTICKER_DEBUG_PIN(running ? 1 : 0);
+ // if nothing has been setup we ignore the ticks
if(!running){
- // if nothing has been setup we ignore the ticks
- if(jobq.empty() || !pop_next_job()) return;
- running= true;
+ // 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;
+ current_tick = 0;
+ current_block= nullptr;
+ 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_job.tick_info[m].steps_to_move == 0) continue; // not active
+ if(current_block->tick_info[m].steps_to_move == 0) continue; // not active
- current_job.tick_info[m].steps_per_tick += current_job.tick_info[m].acceleration_change;
+ current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change;
- if(current_tick == current_job.tick_info[m].next_accel_event) {
- if(current_tick == current_job.block_info.accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau
- current_job.tick_info[m].acceleration_change = 0;
- if(current_job.block_info.decelerate_after < current_job.block_info.total_move_ticks) {
- current_job.tick_info[m].next_accel_event = current_job.block_info.decelerate_after;
- if(current_tick != current_job.block_info.decelerate_after) { // We are plateauing
+ 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_job.tick_info[m].steps_per_tick = current_job.tick_info[m].plateau_rate;
+ current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate;
}
}
}
- if(current_tick == current_job.block_info.decelerate_after) { // We start decelerating
- current_job.tick_info[m].acceleration_change = current_job.tick_info[m].deceleration_change;
+ 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_job.tick_info[m].steps_per_tick <= 0) {
- current_job.tick_info[m].counter = FPSCALE; // we force completion this step by setting to 1.0
- current_job.tick_info[m].steps_per_tick = 0;
+ 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_job.tick_info[m].counter += current_job.tick_info[m].steps_per_tick;
+ current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick;
- if(current_job.tick_info[m].counter >= FPSCALE) { // >= 1.0 step time
- current_job.tick_info[m].counter -= FPSCALE; // -= 1.0F;
- ++current_job.tick_info[m].step_count;
+ 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
- motor[m]->step();
+ 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(current_job.tick_info[m].step_count == current_job.tick_info[m].steps_to_move) {
+ if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) {
// done
- current_job.tick_info[m].steps_to_move = 0;
- motor[m]->moving= false; // let motor know it is no longer moving
+ current_block->tick_info[m].steps_to_move = 0;
+ motor[m]->stop_moving(); // 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;
+ if(motor[m]->is_moving()) still_moving= true;
}
// do this after so we start at tick 0
// see if any motors are still moving
- // FIXME why is this always being called when ther eis nothing todo?
if(!still_moving) {
- stepticker_debug_pin = 0;
+ //SET_STEPTICKER_DEBUG_PIN(0);
// all moves finished
current_tick = 0;
- // get next job
+ // get next block
// do it here so there is no delay in ticks
- // get next job, and copy data
- running= pop_next_job(); // returns false if no new job available
+ 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
}
}
-// pop next job off queue and copy it for faster access
// only called from the step tick ISR (single consumer)
-bool StepTicker::pop_next_job()
+bool StepTicker::start_next_block()
{
- // pop next job, return false if nothing to get
- if(!jobq.get(current_job)) return false;
+ 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_job.tick_info[m].steps_to_move == 0) continue;
+ 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_job.block_info.direction_bits[m]);
- motor[m]->moving= true; // also let motor know it is moving now
+ motor[m]->set_direction(current_block->direction_bits[m]);
+ motor[m]->start_moving(); // also let motor know it is moving now
}
- stepticker_debug_pin = 1;
- return true;
-}
-
-// prepare block for the job queue and push it
-// only called from main_loop() or on_idle() (single producer)
-// this is done ahead of time so does not delay tick generation, see Conveyor::main_loop()
-bool StepTicker::push_block(const Block *block)
-{
- //stepticker_debug_pin = 1;
- job_entry_t job;
+ current_tick= 0;
- job.block_info.accelerate_until = block->accelerate_until;
- job.block_info.decelerate_after = block->decelerate_after;
- job.block_info.total_move_ticks = block->total_move_ticks;
- job.block_info.direction_bits = block->direction_bits;
+ if(ok) {
+ //SET_STEPTICKER_DEBUG_PIN(1);
+ return true;
- float inv = 1.0F / block->steps_event_count;
- for (uint8_t m = 0; m < num_motors; m++) {
- uint32_t steps = block->steps[m];
- job.tick_info[m].steps_to_move = steps;
- if(steps == 0) continue;
-
- float aratio = inv * steps;
- job.tick_info[m].steps_per_tick = TOFP((block->initial_rate * aratio) / frequency); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point
- job.tick_info[m].counter = 0; // 2.30 fixed point
- job.tick_info[m].step_count = 0;
- job.tick_info[m].next_accel_event = block->total_move_ticks + 1;
-
- float acceleration_change = 0;
- if(block->accelerate_until != 0) { // If the next accel event is the end of accel
- job.tick_info[m].next_accel_event = block->accelerate_until;
- acceleration_change = block->acceleration_per_tick;
-
- } else if(block->decelerate_after == 0 /*&& block->accelerate_until == 0*/) {
- // we start off decelerating
- acceleration_change = -block->deceleration_per_tick;
-
- } else if(block->decelerate_after != block->total_move_ticks /*&& block->accelerate_until == 0*/) {
- // If the next event is the start of decel ( don't set this if the next accel event is accel end )
- job.tick_info[m].next_accel_event = block->decelerate_after;
- }
-
- // convert to fixed point after scaling
- job.tick_info[m].acceleration_change= TOFP(acceleration_change * aratio);
- job.tick_info[m].deceleration_change= -TOFP(block->deceleration_per_tick * aratio);
- job.tick_info[m].plateau_rate= TOFP((block->maximum_rate * aratio) / frequency);
+ }else{
+ // this is an edge condition that should never happen, but we need to discard this block if it ever does
+ // basically it is a block that has zero steps for all motors
+ THECONVEYOR->block_finished();
}
- //stepticker_debug_pin = 0;
- return jobq.put(job);
+ return false;
}
+
// returns index of the stepper motor in the array and bitset
int StepTicker::register_motor(StepperMotor* m)
{