DEPFLAGS = -MMD -MP
# Compiler Options
-GPFLAGS = -O$(OPTIMIZATION) -g -mcpu=cortex-m3 -mthumb -mthumb-interwork
+GPFLAGS = -O$(OPTIMIZATION) -g3 -mcpu=cortex-m3 -mthumb -mthumb-interwork
GPFLAGS += -ffunction-sections -fdata-sections -fno-exceptions
GPFLAGS += $(patsubst %,-I%,$(INCDIRS))
GPFLAGS += $(DEFINES)
#include "StepTicker.h"
#include "system_LPC17xx.h" // mbed.h lib
+#include <mri.h>
+
// 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
LPC_TIM1->TCR = 1; // Enable interrupt
// Default start values
+ this->moves_finished = false;
+ this->reset_step_pins = false;
this->debug = 0;
this->has_axes = 0;
this->set_frequency(0.001);
}
}
+void StepTicker::signal_moves_finished(){
+ uint8_t current_id = 0;
+ StepperMotor* current = this->active_motors[0];
+ while(current != NULL ){
+ if( current->is_move_finished ){
+ current->signal_move_finished();
+ if( current->moving == false ){ current_id--; }
+ }
+ current_id++;
+ current = this->active_motors[current_id];
+ }
+ this->moves_finished = false;
+}
+
inline void StepTicker::reset_tick(){
uint8_t current_id = 0;
StepperMotor* current = this->active_motors[0];
while(current != NULL ){
current->step_pin->set(0);
- if( current->exit_tick ){
- if( current->remove_from_active_list_next_tick ){
- current->remove_from_active_list_next_tick = false;
- }else{
+ /*if( current->remove_from_active_list_next_reset ){
+ current->remove_from_active_list_next_reset = false;
this->remove_motor_from_active_list(current);
current_id--;
- }
- }
+ }*/
current_id++;
current = this->active_motors[current_id];
}
global_step_ticker->reset_tick();
}
+
+#pragma GCC push_options
+#pragma GCC optimize ("O0")
+
+
// The actual interrupt handler where we do all the work
extern "C" void TIMER0_IRQHandler (void){
+ uint32_t initial_tc = LPC_TIM0->TC;
+
+ // Do not get out of here before everything is nice and tidy
+
LPC_GPIO1->FIOSET = 1<<18;
- uint32_t start_time = LPC_TIM0->TC;
-
LPC_TIM0->IR |= 1 << 0;
-
- global_step_ticker->debug++;
// If no axes enabled, just ignore for now
- if( !global_step_ticker->has_axes ){
-
+ if( global_step_ticker->active_motors[0] == NULL ){
LPC_GPIO1->FIOCLR = 1<<18;
return;
-
}
- // Do not get out of here before everything is nice and tidy
LPC_TIM0->MR0 = 2000000;
-
+
// Step pins
global_step_ticker->tick();
- LPC_TIM1->TCR = 3;
- LPC_TIM1->TCR = 1;
+ uint32_t after_tick = LPC_TIM0->TC;
+
+ // We may have set a pin on in this tick, now we start the timer to set it off
+ if( global_step_ticker->reset_step_pins ){
+ LPC_TIM1->TCR = 3;
+ LPC_TIM1->TCR = 1;
+ global_step_ticker->reset_step_pins = false;
+ }
+
+ // If a move finished in this tick, we have to tell the actuator to act accordingly
+ if( global_step_ticker->moves_finished ){ global_step_ticker->signal_moves_finished(); }
+
+ uint32_t after_signal = LPC_TIM0->TC;
// If we went over the duration an interrupt is supposed to last, we have a problem
// That can happen tipically when we change blocks, where more than usual computation is done
uint32_t ticks_to_skip = ( ( LPC_TIM0->TC + global_step_ticker->last_duration ) / global_step_ticker->period );
// Next step is now to reduce this to how many steps we can *actually* skip
- uint32_t ticks_we_actually_can_skip = ticks_to_skip;
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)stepper->fx_ticks_per_step - (uint64_t)stepper->fx_counter ) >> 32) ); }
+ uint32_t ticks_we_actually_can_skip = 0;
+ uint8_t current_id = 0; StepperMotor* current = global_step_ticker->active_motors[0];
+ while(current != NULL ){ // For each active stepper
+ ticks_we_actually_can_skip = min( ticks_we_actually_can_skip, (uint32_t)((uint64_t)( (uint64_t)current->fx_ticks_per_step - (uint64_t)current->fx_counter ) >> 32) );
+ current_id++;
+ current = global_step_ticker->active_motors[current_id];
}
- // If the number of ticks we can actually skip is smaller than the number we wanted to skip, there is something wrong in the settings
- //if( ticks_we_actually_can_skip < ticks_to_skip ){ }
-
// Adding to MR0 for this time is not enough, we must also increment the counters ourself artificially
- for(unsigned int i=0; i < global_step_ticker->stepper_motors.size(); i++){
- StepperMotor* stepper = global_step_ticker->stepper_motors[i];
- if( stepper->moving ){ stepper->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32); }
+ current_id = 0; current = global_step_ticker->active_motors[0];
+ while(current != NULL ){ // For each active stepper
+ current->fx_counter += (uint64_t)((uint64_t)(ticks_we_actually_can_skip)<<32);
+ current_id++;
+ current = global_step_ticker->active_motors[current_id];
}
- // When must we have our next MR0 ? ( +1 is here to account that we are actually a legit MR0 too, not only overtime )
- LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
- //LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
+ // When must we have our next MR0 ? ( +1 is here to account that we are actually doing a legit MR0 match here too, not only overtime )
+ // LPC_TIM0->MR0 = ( ticks_we_actually_can_skip + 1 ) * global_step_ticker->period;
+ LPC_TIM0->MR0 = ( ticks_to_skip + 1 ) * global_step_ticker->period;
// This is so that we know how long this computation takes, and we can take it into account next time
- global_step_ticker->last_duration = LPC_TIM0->TC - start_tc;
+ int difference = (int)(LPC_TIM0->TC) - (int)(start_tc);
+ if( difference > 0 ){ global_step_ticker->last_duration = (uint32_t)difference; }
+ if( global_step_ticker->last_duration > 10000 || LPC_TIM0->MR0 > 10000 || LPC_TIM0->TC > 10000 || initial_tc > 10000 || after_tick > 10000 || after_signal > 10000 ){ __debugbreak(); }
}else{
LPC_TIM0->MR0 = global_step_ticker->period;
}
-
-
- if( LPC_TIM0->TC > LPC_TIM0->MR0 ){
- // LPC_TIM0->TCR = 3; // Reset
- // LPC_TIM0->TCR = 1; // Reset
- LPC_TIM0->TC = LPC_TIM0->MR0 - 2;
- }
-
-
LPC_GPIO1->FIOCLR = 1<<18;
+
+ while( LPC_TIM0->TC > LPC_TIM0->MR0 ){
+ LPC_TIM0->MR0 += global_step_ticker->period;
+ }
}
+#pragma GCC pop_options
+
// We make a list of steppers that want to be called so that we don't call them for nothing
void StepTicker::add_motor_to_active_list(StepperMotor* motor){
- //printf("adding %p with exit: %u \r\n", motor, motor->exit_tick );
uint8_t current_id = 0;
StepperMotor* current = this->active_motors[0];
while(current != NULL ){
}
void StepTicker::remove_motor_from_active_list(StepperMotor* motor){
- //printf("removing %p with exit: %u \r\n", motor, motor->exit_tick );
uint8_t current_id = 0;
uint8_t offset = 0;
StepperMotor* current = this->active_motors[0];
StepTicker();
void set_frequency( double frequency );
void tick();
+ void signal_moves_finished();
StepperMotor* add_stepper_motor(StepperMotor* stepper_motor);
void set_reset_delay( double seconds );
void reset_tick();
uint32_t last_duration;
bool has_axes;
+ bool moves_finished;
+ bool reset_step_pins;
+
StepperMotor* active_motors[12];
};
this->fx_ticks_per_step = 0;
this->steps_to_move = 0;
this->direction_bit = 0;
- this->step_bit = 0;
- this->update_exit_tick();
- this->remove_from_active_list_next_tick = false;
- this->exit_tick = true;
+ //this->update_exit_tick();
+ this->remove_from_active_list_next_reset = false;
+ this->is_move_finished = false;
}
StepperMotor::StepperMotor(Pin* step, Pin* dir, Pin* en) : step_pin(step), dir_pin(dir), en_pin(en) {
this->fx_ticks_per_step = 0;
this->steps_to_move = 0;
this->direction_bit = 0;
- this->step_bit = 0;
- this->update_exit_tick();
- this->remove_from_active_list_next_tick = false;
- this->exit_tick = true;
+ //this->update_exit_tick();
+ this->remove_from_active_list_next_reset = false;
+ this->is_move_finished = false;
}
// Called a great many times per second, to step if we have to now
bool StepperMotor::tick(){
-
- // output to pins 37t
- this->dir_pin->set( this->direction_bit );
- this->step_pin->set( this->step_bit );
-
- // ignore inactive steppers 13t
- if( this->exit_tick ){
- this->step_bit = 0;
- return false;
- }
-
// increase the ( fixed point ) counter by one tick 11t
this->fx_counter += (uint64_t)((uint64_t)1<<32);
// if we are to step now 10t
if( this->fx_counter >= this->fx_ticks_per_step ){
+ // output to pins 37t
+ this->dir_pin->set( this->direction_bit );
+ this->step_pin->set( 1 );
+ this->step_ticker->reset_step_pins = true;
// move counter back 11t
this->fx_counter -= this->fx_ticks_per_step;
- // we must step, actual output is done at the beginning of this function 8t
- this->step_bit = 1;
-
// we have moved a step 9t
this->stepped++;
- //printf("%u %u %u %u %u %u \r\n", this->moving, this->paused, this->fx_ticks_per_step == 0, this->steps_to_move == 0, this->stepped, this->steps_to_move );
+ // is this move finished ? 11t
+ if( this->stepped == this->steps_to_move ){
+ this->is_move_finished = true;
+ this->step_ticker->moves_finished = true;
+ }
+ }
- // is this move finished ? 11t
- if( this->stepped == this->steps_to_move ){
+}
- //printf("end\r\n");
+void StepperMotor::signal_move_finished(){
// work is done ! 8t
this->moving = false;
this->steps_to_move = 0;
- this->update_exit_tick();
// signal it to whatever cares 41t 411t
this->end_hook->call();
+ // We only need to do this if we were not instructed to move
+ if( this->moving == false ){
+ this->update_exit_tick();
+ }
- }
-
- }else{
-
- // we must not step
- this->step_bit = 0;
- }
+ this->is_move_finished = false;
}
//printf("try update list %u %u %u\r\n", this->moving, this->paused, this->fx_ticks_per_step == 0 );
if( !this->moving || this->paused || this->steps_to_move == 0 ){
// We must exit tick() after setting the pins, no bresenham is done
- if( this->exit_tick == false ){
- this->exit_tick = true;
- this->remove_from_active_list_next_tick = true;
- }
+ //this->remove_from_active_list_next_reset = true;
+ this->step_ticker->remove_motor_from_active_list(this);
}else{
// We must do the bresenham in tick()
- if( this->exit_tick == true ){
- // We have to do this or there could be a bug where the removal still happens when it doesn't need to
- this->remove_from_active_list_next_tick = false;
-
- this->exit_tick = false;
- this->step_ticker->add_motor_to_active_list(this);
- }
+ // We have to do this or there could be a bug where the removal still happens when it doesn't need to
+ //this->remove_from_active_list_next_reset = false;
+ this->step_ticker->add_motor_to_active_list(this);
}
}
StepperMotor();
StepperMotor(Pin* step, Pin* dir, Pin* en);
bool tick();
+ void move_finished();
void move( bool direction, unsigned int steps );
+ void signal_move_finished();
void set_speed( double speed );
void update_exit_tick();
void pause();
void unpause();
+
template<typename T> void attach( T *optr, uint32_t ( T::*fptr )( uint32_t ) ){
Hook* hook = new Hook();
hook->attach(optr, fptr);
bool paused;
bool direction_bit;
- bool step_bit;
+ //bool step_bit;
uint32_t steps_to_move;
uint32_t stepped;
uint64_t fx_counter;
uint64_t fx_ticks_per_step;
- bool exit_tick;
- bool remove_from_active_list_next_tick;
+ //bool exit_tick;
+ bool remove_from_active_list_next_reset;
+ bool is_move_finished; // Whether the move just finished
};
}
-#pragma GCC push_options
-#pragma GCC optimize ("O0")
+//#pragma GCC push_options
+//#pragma GCC optimize ("O0")
// When a stepper motor has finished it's assigned movement
uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy){
// This block is finished, release it
if( this->current_block != NULL ){
-
-
this->current_block->release();
-
-
}
}
if(this->current_block && !this->paused && this->main_stepper->moving ) {
uint32_t current_steps_completed = this->main_stepper->stepped;
-
- if( current_steps_completed >= this->main_stepper->steps_to_move - 3 ){
- }
if( previous_step_count == current_steps_completed && previous_step_count != 0 ){
// We must skip this step update because no step has happened
if( this->force_speed_update ){
this->force_speed_update = false;
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
+ return 0;
}
if(current_steps_completed <= this->current_block->accelerate_until + 1) {
-
- /*
- if( this->current_block->initial_rate == 0 && 0 ){
- __disable_irq();uint32_t start_tc0 = LPC_TIM0->TC; uint32_t start_tc2 = LPC_TIM2->TC;
- this->kernel->streams->printf("acc{%u<%u} tar:%f im:%u sps:%f pos:%u/%u cnt:%u delta:%f \r\n", current_steps_completed, this->current_block->accelerate_until, this->trapezoid_adjusted_rate, this->main_stepper->moving, this->main_stepper->steps_per_second, this->main_stepper->stepped, this->main_stepper->steps_to_move, (uint32_t)(this->main_stepper->fx_ticks_per_step>>32), this->current_block->rate_delta );
- LPC_TIM0->TC = start_tc0; LPC_TIM2->TC = start_tc2; __enable_irq();
- }
- */
-
this->trapezoid_adjusted_rate += ( skipped_speed_updates + 1 ) * this->current_block->rate_delta;
if (this->trapezoid_adjusted_rate > this->current_block->nominal_rate ) {
-
- //this->kernel->streams->printf("reached after %u with au %u and da %u \r\n", current_steps_completed, this->current_block->accelerate_until, this->current_block->decelerate_after);
-
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
-
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
-
-
}else if (current_steps_completed > this->current_block->decelerate_after + 1) {
-
- LPC_GPIO1->FIOSET = 1<<20;
- /*
- if( this->current_block->final_rate == 0 && 0 ){
- __disable_irq();uint32_t start_tc0 = LPC_TIM0->TC; uint32_t start_tc2 = LPC_TIM2->TC;
- this->kernel->streams->printf("dec{%u>=%u} tar:%f im:%u sps:%f pos:%u/%u cnt:%u delta:%f\r\n", current_steps_completed, this->current_block->decelerate_after, this->trapezoid_adjusted_rate, this->main_stepper->moving, this->main_stepper->steps_per_second, this->main_stepper->stepped, this->main_stepper->steps_to_move, (uint32_t)(this->main_stepper->fx_ticks_per_step>>32), this->current_block->rate_delta );
- LPC_TIM0->TC = start_tc0; LPC_TIM2->TC = start_tc2; __enable_irq();
- }
- */
+ LPC_GPIO1->FIOSET = 1<<20;
// NOTE: We will only reduce speed if the result will be > 0. This catches small
// rounding errors that might leave steps hanging after the last trapezoid tick.
}
this->set_step_events_per_minute(this->trapezoid_adjusted_rate);
-
- LPC_GPIO1->FIOCLR = 1<<20;
-
+ LPC_GPIO1->FIOCLR = 1<<20;
}else {
// Make sure we cruise at exactly nominal rate
if (this->trapezoid_adjusted_rate != this->current_block->nominal_rate) {
}
-#pragma GCC pop_options
+//#pragma GCC pop_options
// Initializes the trapezoid generator from the current block. Called whenever a new