#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;
this->unstep.reset();
this->num_motors = 0;
- this->move_issued = false;
+ 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()
if(finished_fnc) finished_fnc();
}
-
// step clock
void StepTicker::step_tick (void)
{
static uint32_t current_tick = 0;
- if(!move_issued){
- if(jobq.empty()) return; // if nothing has been setup we ignore the ticks
- // get next job, and copy data
- Block *b= nullptr;
- jobq.get(b);
- copy_block(b);
- }
+ //SET_STEPTICKER_DEBUG_PIN(running ? 1 : 0);
- current_tick++; // count number of ticks
+ // 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;
+ }
+ }
- bool still_moving = false;
+ 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(tick_info[m].steps_to_move == 0) continue; // not active
+ if(current_block->tick_info[m].steps_to_move == 0) continue; // not active
- still_moving = true;
- tick_info[m].steps_per_tick += tick_info[m].acceleration_change;
+ current_block->tick_info[m].steps_per_tick += current_block->tick_info[m].acceleration_change;
- if(current_tick == tick_info[m].next_accel_event) {
- if(current_tick == block_info.accelerate_until) { // We are done accelerating, deceleration becomes 0 : plateau
- tick_info[m].acceleration_change = 0;
- if(block_info.decelerate_after < block_info.total_move_ticks) {
- tick_info[m].next_accel_event = block_info.decelerate_after;
- if(current_tick != 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
- tick_info[m].steps_per_tick = tick_info[m].plateau_rate;
+ current_block->tick_info[m].steps_per_tick = current_block->tick_info[m].plateau_rate;
}
}
}
- if(current_tick == block_info.decelerate_after) { // We start decelerating
- tick_info[m].acceleration_change = 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(tick_info[m].steps_per_tick <= 0) {
- tick_info[m].counter = (1<<30); // we complete this step
- 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;
}
- tick_info[m].counter += tick_info[m].steps_per_tick;
+ current_block->tick_info[m].counter += current_block->tick_info[m].steps_per_tick;
- if(tick_info[m].counter >= (1<<30)) { // > 1.0 step time
- tick_info[m].counter -= (1<<30); // -= 1.0F;
- ++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(tick_info[m].step_count == tick_info[m].steps_to_move) {
+ if(!ismoving || current_block->tick_info[m].step_count == current_block->tick_info[m].steps_to_move) {
// done
- tick_info[m].steps_to_move = 0;
+ 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
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 static block and tick info from next block
+ // get next block
// do it here so there is no delay in ticks
- if(!jobq.empty()) {
- #ifdef STEPTICKER_DEBUG_PIN
- stepticker_debug_pin = 1;
- #endif
-
- // get next job, and copy data
- Block *b= nullptr;
- jobq.get(b);
- copy_block(b);
-
- #ifdef STEPTICKER_DEBUG_PIN
- stepticker_debug_pin = 0;
- #endif
-
- } else {
- move_issued = false; // nothing to do as no more blocks
+ 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;
+ //SCB->ICSR = 0x10000000; // SCB_ICSR_PENDSVSET_Msk;
}
}
-// called in ISR if running, else can be called from anything to start
-void StepTicker::copy_block(Block *block)
+// only called from the step tick ISR (single consumer)
+bool StepTicker::start_next_block()
{
- stepticker_debug_pin = 1;
- block_info.accelerate_until = block->accelerate_until;
- block_info.decelerate_after = block->decelerate_after;
- block_info.total_move_ticks = block->total_move_ticks;
+ if(current_block == nullptr) return false;
- float inv = 1.0F / block->steps_event_count;
+ bool ok= false;
+ // need to prepare each active motor
for (uint8_t m = 0; m < num_motors; m++) {
- uint32_t steps = block->steps[m];
- tick_info[m].steps_to_move = steps;
- if(steps == 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
- motor[m]->set_direction(block->direction_bits[m]);
-
- float aratio = inv * steps;
- tick_info[m].steps_per_tick = floorf(((block->initial_rate * aratio) / frequency) * (1<<30)); // steps/sec / tick frequency to get steps per tick in 2.30 fixed point
- tick_info[m].counter = 0; // 2.30 fixed point
- tick_info[m].step_count = 0;
- 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
- 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 )
- tick_info[m].next_accel_event = block->decelerate_after;
- }
+ // 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
+ }
- // convert to fixed point after scaling
- tick_info[m].acceleration_change= floorf((acceleration_change * aratio) * (1<<30));
- tick_info[m].deceleration_change= -floorf((block->deceleration_per_tick * aratio) * (1<<30));
- tick_info[m].plateau_rate= floorf(((block->maximum_rate * aratio) / frequency) * (1<<30));
+ if(ok) {
+ SET_STEPTICKER_DEBUG_PIN(1);
+ return true;
}
- move_issued = true;
- stepticker_debug_pin = 0;
+
+ return false;
}
+
// returns index of the stepper motor in the array and bitset
int StepTicker::register_motor(StepperMotor* m)
{
HCoop / clinton / Smoothieware.git / blobdiff