If the stepper moved in the last block base next step timing on the last step time for all motors,
this is different as before it was based on the time of the last motor to finish, and was not compensated
this->set_acceleration_ticks_per_second(1000);
this->num_motors= 0;
this->active_motor.reset();
- this->acceleration_tick_cnt= 0;
- this->do_acceleration_tick= false;
+ this->tick_cnt= 0;
}
StepTicker::~StepTicker() {
// this is the number of acceleration ticks per second
void StepTicker::set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second) {
- this->acceleration_tick_period= floorf(this->frequency/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;
stepticker_debug_pin= 0;
#endif
}
-
- // if(this->do_acceleration_tick) {
- // this->do_acceleration_tick= false;
- // // call registered acceleration handlers
- // for (size_t i = 0; i < acceleration_tick_handlers.size(); ++i) {
- // acceleration_tick_handlers[i]();
- // }
- // }
}
-// run in RIT
+// run in RIT lower priority than PendSV
void StepTicker::acceleration_tick() {
//stepticker_debug_pin= 1;
// call registered acceleration handlers
void StepTicker::TIMER0_IRQHandler (void){
// Reset interrupt register
LPC_TIM0->IR |= 1 << 0;
+ tick_cnt++; // count number of ticks
// 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++){
- // always send tick to all motors
- this->motor[motor]->tick();
+ // send tick to all active motors
+ if(this->active_motor[motor])
+ this->motor[motor]->tick();
}
// We may have set a pin on in this tick, now we start the timer to set it off
this->reset_step_pins = false;
}
- // do an acceleration tick, after we have done everything else (run in ON_IDLE)
- // if(++this->acceleration_tick_cnt >= this->acceleration_tick_period) {
- // this->acceleration_tick_cnt= 0;
- // this->do_acceleration_tick= true;
- // }
-
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
void set_acceleration_ticks_per_second(uint32_t acceleration_ticks_per_second);
float get_frequency() const { return frequency; }
void reset_tick();
+ uint32_t get_tick_cnt() const { return tick_cnt; }
+ uint32_t ticks_since(uint32_t last) const { return (tick_cnt>last) ? tick_cnt-last : (UINT32_MAX-last) + tick_cnt + 1; }
+
void TIMER0_IRQHandler (void);
void PendSV_IRQHandler (void);
-
void register_acceleration_tick_handler(std::function<void(void)> cb){
acceleration_tick_handlers.push_back(cb);
}
float frequency;
uint32_t delay;
uint32_t period;
- uint32_t acceleration_tick_period;
- uint32_t acceleration_tick_cnt;
+ volatile uint32_t tick_cnt;
std::vector<std::function<void(void)>> acceleration_tick_handlers;
std::vector<StepperMotor*> motor;
std::bitset<32> active_motor; // limit to 32 motors
std::atomic_uchar do_move_finished;
uint8_t num_motors;
volatile bool a_move_finished;
- volatile bool do_acceleration_tick;
volatile bool reset_step_pins;
};
float StepperMotor::default_minimum_actuator_rate= 20.0F;
// A StepperMotor represents an actual stepper motor. It is used to generate steps that move the actual motor at a given speed
-// TODO : Abstract this into Actuator
StepperMotor::StepperMotor()
{
this->steps_to_move = 0;
this->is_move_finished = false;
this->active= false;
+ this->last_step_tick_valid= false;
steps_per_mm = 1.0F;
max_rate = 50.0F;
// This is called ( see the .h file, we had to put a part of things there for obscure inline reasons ) when a step has to be generated
// we also here check if the move is finished etc ..
-// Thi sis in highest priority interrupt so cannot be pre-empted
+// This is in highest priority interrupt so cannot be pre-empted
void StepperMotor::step()
{
- // we can't do anything until the next move has been processed, but we will be able to offset the time by shortening the next step
- if(!this->active) return;
+ // we can't do anything until the next move has been processed
+ if(!this->active || !this->moving) return;
// output to pins 37t
this->step_pin.set( 1 );
// This is so we don't call that before all the steps have been generated for this tick()
this->is_move_finished = true;
THEKERNEL->step_ticker->a_move_finished= true;
+ this->last_step_tick= THEKERNEL->step_ticker->get_tick_cnt(); // remember when last step was
}
}
void StepperMotor::signal_move_finished()
{
// work is done ! 8t
- this->fx_counter = 0; // set this to zero here so we don't miss any for next move
- this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
this->moving = false;
this->steps_to_move = 0;
this->minimum_step_rate = default_minimum_actuator_rate;
- // signal it to whatever cares 41t 411t
+ // signal it to whatever cares
+ // in this call a new block may start, new moves set and new speeds
this->end_hook->call();
// We only need to do this if we were not instructed to move
- // FIXME trouble here is that if this stops first but other axis are still going we will now stop getting ticks so we cannot know how long we were delayed
- if( this->moving == false ) {
+ if( !this->moving ) {
this->update_exit_tick();
}
}
// Instruct the StepperMotor to move a certain number of steps
-void StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
+StepperMotor* StepperMotor::move( bool direction, unsigned int steps, float initial_speed)
{
this->dir_pin.set(direction);
this->direction = direction;
// Zero our tool counters
this->stepped = 0;
+ this->fx_ticks_per_step = 0xFFFFF000UL; // some big number so we don't start stepping before it is set again
+ if(this->last_step_tick_valid) {
+ // NOTE only steppermotors managed in Stepper.cpp set or unset this flag
+ // we set this based on when the last step was, thus compensating for missed ticks
+ this->fx_counter= THEKERNEL->step_ticker->ticks_since(this->last_step_tick)*fx_increment;
+ }else{
+ this->fx_counter = 0; // set to zero as there was no step last block
+ }
// Starting now we are moving
if( steps > 0 ) {
this->moving = false;
}
this->update_exit_tick();
+ return this;
}
// Set the speed at which this stepper moves in steps/sec, should be called set_step_rate()
// we need to make sure that we have a minimum speed here and that it fits the 32bit fixed point fx counters
// Note nothing will really ever go as slow as the minimum speed here, it is just forced to avoid bad errors
// fx_ticks_per_step is what actually sets the step rate, it is fixed point 18.14
-void StepperMotor::set_speed( float speed )
+StepperMotor* StepperMotor::set_speed( float speed )
{
if(speed < minimum_step_rate) {
speed= minimum_step_rate;
// set the new speed, NOTE this can be pre-empted by stepticker so the following write needs to be atomic
this->fx_ticks_per_step= floor(fx_increment * THEKERNEL->step_ticker->get_frequency() / speed);
+ return this;
}
// Pause this stepper motor
bool is_moving() { return moving; }
void move_finished();
- void move( bool direction, unsigned int steps, float initial_speed= -1.0F);
+ StepperMotor* move( bool direction, unsigned int steps, float initial_speed= -1.0F);
void signal_move_finished();
- void set_speed( float speed );
-
+ StepperMotor* set_speed( float speed );
+ void set_moved_last_block(bool flg) { last_step_tick_valid= flg; }
void update_exit_tick();
void pause();
void unpause();
uint32_t steps_to_move;
uint32_t stepped;
+ uint32_t last_step_tick;
// set to 32 bit fixed point, 18:14 bits fractional
static const uint32_t fx_shift= 14;
- static const uint32_t fx_increment= ((uint64_t)1<<fx_shift);
+ static const uint32_t fx_increment= ((uint32_t)1<<fx_shift);
uint32_t fx_counter;
uint32_t fx_ticks_per_step;
bool paused:1;
bool active:1; // whether in the stepticker active motors list or not
volatile bool moving:1;
+ bool last_step_tick_valid:1; // set if the last step tick time is valid (ie the motor moved last block)
};
// Called a great many times per second, to step if we have to now
}
// Setup : instruct stepper motors to move
+ // Find the stepper with the more steps, it's the one the speed calculations will want to follow
+ this->main_stepper= nullptr;
if( block->steps[ALPHA_STEPPER] > 0 ) {
- THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER]);
+ THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER])->set_moved_last_block(true);
+ this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
+ }else{
+ THEKERNEL->robot->alpha_stepper_motor->set_moved_last_block(false);
}
+
if( block->steps[BETA_STEPPER ] > 0 ) {
- THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ]);
+ THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ])->set_moved_last_block(true);
+ if(this->main_stepper == nullptr || THEKERNEL->robot->beta_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
+ this->main_stepper = THEKERNEL->robot->beta_stepper_motor;
+ }else{
+ THEKERNEL->robot->beta_stepper_motor->set_moved_last_block(false);
}
if( block->steps[GAMMA_STEPPER] > 0 ) {
- THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER]);
+ THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER])->set_moved_last_block(true);
+ if(this->main_stepper == nullptr || THEKERNEL->robot->gamma_stepper_motor->get_steps_to_move() > this->main_stepper->get_steps_to_move())
+ this->main_stepper = THEKERNEL->robot->gamma_stepper_motor;
+ }else{
+ THEKERNEL->robot->gamma_stepper_motor->set_moved_last_block(false);
}
this->current_block = block;
// Setup acceleration for this block
this->trapezoid_generator_reset();
- // Find the stepper with the more steps, it's the one the speed calculations will want to follow
- this->main_stepper = THEKERNEL->robot->alpha_stepper_motor;
- if( THEKERNEL->robot->beta_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ) {
- this->main_stepper = THEKERNEL->robot->beta_stepper_motor;
- }
- if( THEKERNEL->robot->gamma_stepper_motor->steps_to_move > this->main_stepper->steps_to_move ) {
- this->main_stepper = THEKERNEL->robot->gamma_stepper_motor;
- }
-
// Set the initial speed for this move
this->trapezoid_generator_tick();
}
HCoop / clinton / Smoothieware.git / commitdiff