#include "ConfigValue.h"
#include "Gcode.h"
#include "Block.h"
+#include "StepTicker.h"
#include <vector>
using namespace std;
#include <mri.h>
-#define acceleration_ticks_per_second_checksum CHECKSUM("acceleration_ticks_per_second")
-#define minimum_steps_per_minute_checksum CHECKSUM("minimum_steps_per_minute")
-
// The stepper reacts to blocks that have XYZ movement to transform them into actual stepper motor moves
// TODO: This does accel, accel should be in StepperMotor
Stepper::Stepper()
{
this->current_block = NULL;
- this->paused = false;
- this->trapezoid_generator_busy = false;
this->force_speed_update = false;
this->halted= false;
}
this->register_for_event(ON_BLOCK_END);
this->register_for_event(ON_GCODE_EXECUTE);
this->register_for_event(ON_GCODE_RECEIVED);
- this->register_for_event(ON_PLAY);
- this->register_for_event(ON_PAUSE);
this->register_for_event(ON_HALT);
// Get onfiguration
this->on_config_reload(this);
// Acceleration ticker
- this->acceleration_tick_hook = THEKERNEL->slow_ticker->attach( this->acceleration_ticks_per_second, this, &Stepper::trapezoid_generator_tick );
+ THEKERNEL->step_ticker->register_acceleration_tick_handler([this](){trapezoid_generator_tick(); });
// Attach to the end_of_move stepper event
- THEKERNEL->robot->alpha_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
- THEKERNEL->robot->beta_stepper_motor->attach( this, &Stepper::stepper_motor_finished_move );
- THEKERNEL->robot->gamma_stepper_motor->attach(this, &Stepper::stepper_motor_finished_move );
+ for (auto actuator : THEKERNEL->robot->actuators)
+ actuator->attach(this, &Stepper::stepper_motor_finished_move );
}
// Get configuration from the config file
void Stepper::on_config_reload(void *argument)
{
-
- this->acceleration_ticks_per_second = THEKERNEL->config->value(acceleration_ticks_per_second_checksum)->by_default(100 )->as_number();
- this->minimum_steps_per_second = THEKERNEL->config->value(minimum_steps_per_minute_checksum )->by_default(120 )->as_number() / 60.0F;
-
// Steppers start off by default
this->turn_enable_pins_off();
}
-// When the play/pause button is set to pause, or a module calls the ON_PAUSE event
-void Stepper::on_pause(void *argument)
-{
- this->paused = true;
- THEKERNEL->robot->alpha_stepper_motor->pause();
- THEKERNEL->robot->beta_stepper_motor->pause();
- THEKERNEL->robot->gamma_stepper_motor->pause();
-}
-
-// When the play/pause button is set to play, or a module calls the ON_PLAY event
-void Stepper::on_play(void *argument)
-{
- // TODO: Re-compute the whole queue for a cold-start
- this->paused = false;
- THEKERNEL->robot->alpha_stepper_motor->unpause();
- THEKERNEL->robot->beta_stepper_motor->unpause();
- THEKERNEL->robot->gamma_stepper_motor->unpause();
-}
-
void Stepper::on_halt(void *argument)
{
if(argument == nullptr) {
// Attach gcodes to the last block for on_gcode_execute
if( gcode->has_m && (gcode->m == 84 || gcode->m == 17 || gcode->m == 18 )) {
THEKERNEL->conveyor->append_gcode(gcode);
-
}
}
// Enable steppers
void Stepper::turn_enable_pins_on()
{
- for (StepperMotor *m : THEKERNEL->robot->actuators)
- m->enable(true);
+ for (auto a : THEKERNEL->robot->actuators)
+ a->enable(true);
this->enable_pins_status = true;
+ THEKERNEL->call_event(ON_ENABLE, (void*)1);
}
// Disable steppers
void Stepper::turn_enable_pins_off()
{
- for (StepperMotor *m : THEKERNEL->robot->actuators)
- m->enable(false);
+ for (auto a : THEKERNEL->robot->actuators)
+ a->enable(false);
this->enable_pins_status = false;
+ THEKERNEL->call_event(ON_ENABLE, nullptr);
}
// A new block is popped from the queue
{
Block *block = static_cast<Block *>(argument);
- // The stepper does not care about 0-blocks
- if( block->millimeters == 0.0F ) {
- return;
+ // Mark the new block as of interrest to us, handle blocks that have no axis moves properly (like Extrude blocks etc)
+ bool take = false;
+ if (block->millimeters > 0.0F) {
+ for (size_t s = 0; !take && s < THEKERNEL->robot->actuators.size(); s++) {
+ take = block->steps[s] > 0;
+ }
}
-
- // Mark the new block as of interrest to us
- if( block->steps[ALPHA_STEPPER] > 0 || block->steps[BETA_STEPPER] > 0 || block->steps[GAMMA_STEPPER] > 0 ) {
+ if (take){
block->take();
} else {
+ // none of the steppers move this block so make sure they know that
+ for(auto a : THEKERNEL->robot->actuators) {
+ a->set_moved_last_block(false);
+ }
return;
}
}
// Setup : instruct stepper motors to move
- if( block->steps[ALPHA_STEPPER] > 0 ) {
- THEKERNEL->robot->alpha_stepper_motor->move( block->direction_bits[ALPHA_STEPPER], block->steps[ALPHA_STEPPER], 0 );
- }
- if( block->steps[BETA_STEPPER ] > 0 ) {
- THEKERNEL->robot->beta_stepper_motor->move( block->direction_bits[BETA_STEPPER], block->steps[BETA_STEPPER ], 0 );
- }
- if( block->steps[GAMMA_STEPPER] > 0 ) {
- THEKERNEL->robot->gamma_stepper_motor->move( block->direction_bits[GAMMA_STEPPER], block->steps[GAMMA_STEPPER], 0 );
+ // Find the stepper with the more steps, it's the one the speed calculations will want to follow
+ this->main_stepper = nullptr;
+ int most_steps_to_move = 0;
+ for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+ if (block->steps[i] > 0) {
+ THEKERNEL->robot->actuators[i]->move(block->direction_bits[i], block->steps[i])->set_moved_last_block(true);
+ int steps_to_move = THEKERNEL->robot->actuators[i]->get_steps_to_move();
+ if (steps_to_move > most_steps_to_move) {
+ most_steps_to_move = steps_to_move;
+ this->main_stepper = THEKERNEL->robot->actuators[i];
+ }
+ }
+ else {
+ THEKERNEL->robot->actuators[i]->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(0);
+ this->trapezoid_generator_tick();
- // Synchronise the acceleration curve with the stepping
- this->synchronize_acceleration(0);
+ // synchronize the acceleration timer with the start of the new block so it does not drift and randomly fire during the block
+ THEKERNEL->step_ticker->synchronize_acceleration(false);
+ // set a flag to synchronize the acceleration timer with the deceleration step, and fire it immediately we get to that step
+ if( block->decelerate_after > 0 && block->decelerate_after+1 < this->main_stepper->steps_to_move ) {
+ this->main_stepper->signal_step= block->decelerate_after+1; // we make it +1 as deceleration does not start until steps > decelerate_after
+ }
}
// Current block is discarded
// When a stepper motor has finished it's assigned movement
uint32_t Stepper::stepper_motor_finished_move(uint32_t dummy)
{
-
// We care only if none is still moving
- if( THEKERNEL->robot->alpha_stepper_motor->moving || THEKERNEL->robot->beta_stepper_motor->moving || THEKERNEL->robot->gamma_stepper_motor->moving ) {
- return 0;
+ for (auto a : THEKERNEL->robot->actuators) {
+ if(a->moving)
+ return 0;
}
// This block is finished, release it
// This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
// interrupt. It can be assumed that the trapezoid-generator-parameters and the
// current_block stays untouched by outside handlers for the duration of this function call.
-uint32_t Stepper::trapezoid_generator_tick( uint32_t dummy )
+// NOTE caled at the same priority as PendSV so it may make that longer but it is better that having htis pre empted by pendsv
+void Stepper::trapezoid_generator_tick(void)
{
-
// Do not do the accel math for nothing
- if(this->current_block && !this->paused && this->main_stepper->moving ) {
+ if(this->current_block && this->main_stepper->moving ) {
// Store this here because we use it a lot down there
uint32_t current_steps_completed = this->main_stepper->stepped;
+ float last_rate= trapezoid_adjusted_rate;
if( this->force_speed_update ) {
// Do not accel, just set the value
this->force_speed_update = false;
+ last_rate= -1;
} else if(THEKERNEL->conveyor->is_flushing()) {
// if we are flushing the queue, decelerate to 0 then finish this block
trapezoid_adjusted_rate -= current_block->rate_delta;
} else if (trapezoid_adjusted_rate == current_block->rate_delta * 0.5F) {
- for (auto i : THEKERNEL->robot->actuators)
- i->move(i->direction, 0, 0);
- if (current_block)
- current_block->release();
- return 0;
+ for (auto i : THEKERNEL->robot->actuators) i->move(i->direction, 0); // stop motors
+ if (current_block) current_block->release();
+ THEKERNEL->call_event(ON_SPEED_CHANGE, 0); // tell others we stopped
+ return;
+
} else {
trapezoid_adjusted_rate = current_block->rate_delta * 0.5F;
}
- } else if(current_steps_completed < this->current_block->accelerate_until) {
+ } else if(current_steps_completed <= this->current_block->accelerate_until) {
// If we are accelerating
// Increase speed
this->trapezoid_adjusted_rate += this->current_block->rate_delta;
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
}
- } else if (current_steps_completed >= this->current_block->decelerate_after) {
+ } else if (current_steps_completed > this->current_block->decelerate_after) {
// If we are decelerating
// Reduce speed
// NOTE: We will only reduce speed if the result will be > 0. This catches small
this->trapezoid_adjusted_rate = this->current_block->nominal_rate;
}
- this->set_step_events_per_second(this->trapezoid_adjusted_rate);
+ if(last_rate != trapezoid_adjusted_rate) {
+ // don't call this if speed did not change
+ this->set_step_events_per_second(this->trapezoid_adjusted_rate);
+ }
}
-
- return 0;
}
// Initializes the trapezoid generator from the current block. Called whenever a new
// Update the speed for all steppers
void Stepper::set_step_events_per_second( float steps_per_second )
{
- // We do not step slower than this, FIXME shoul dbe calculated for the slowest axis not the fastest
- //steps_per_second = max(steps_per_second, this->minimum_steps_per_second);
- if( steps_per_second < this->minimum_steps_per_second ) {
- steps_per_second = this->minimum_steps_per_second;
- }
+ float isps= steps_per_second / this->current_block->steps_event_count;
// Instruct the stepper motors
- if( THEKERNEL->robot->alpha_stepper_motor->moving ) {
- THEKERNEL->robot->alpha_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[ALPHA_STEPPER] / (float)this->current_block->steps_event_count ) );
- }
- if( THEKERNEL->robot->beta_stepper_motor->moving ) {
- THEKERNEL->robot->beta_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[BETA_STEPPER ] / (float)this->current_block->steps_event_count ) );
- }
- if( THEKERNEL->robot->gamma_stepper_motor->moving ) {
- THEKERNEL->robot->gamma_stepper_motor->set_speed( steps_per_second * ( (float)this->current_block->steps[GAMMA_STEPPER] / (float)this->current_block->steps_event_count ) );
+ for (size_t i = 0; i < THEKERNEL->robot->actuators.size(); i++) {
+ if (THEKERNEL->robot->actuators[i]->moving) {
+ THEKERNEL->robot->actuators[i]->set_speed(isps * this->current_block->steps[i]);
+ }
}
// Other modules might want to know the speed changed
THEKERNEL->call_event(ON_SPEED_CHANGE, this);
-
}
-// This function has the role of making sure acceleration and deceleration curves have their
-// rhythm synchronized. The accel/decel must start at the same moment as the speed update routine
-// This is caller in "step just occured" or "block just began" ( step Timer ) context, so we need to be fast.
-// All we do is reset the other timer so that it does what we want
-uint32_t Stepper::synchronize_acceleration(uint32_t dummy)
-{
-
- // No move was done, this is called from on_block_begin
- // This means we setup the accel timer in a way where it gets called right after
- // we exit this step interrupt, and so that it is then in synch with
- if( this->main_stepper->stepped == 0 ) {
- // Whatever happens, we must call the accel interrupt asap
- // Because it will set the initial rate
- // We also want to synchronize in case we start accelerating or decelerating now
-
- // Accel interrupt must happen asap
- NVIC_SetPendingIRQ(TIMER2_IRQn);
- // Synchronize both counters
- LPC_TIM2->TC = LPC_TIM0->TC;
-
- // If we start decelerating after this, we must ask the actuator to warn us
- // so we can do what we do in the "else" bellow
- if( this->current_block->decelerate_after > 0 && this->current_block->decelerate_after < this->main_stepper->steps_to_move ) {
- this->main_stepper->attach_signal_step(this->current_block->decelerate_after, this, &Stepper::synchronize_acceleration);
- }
- } else {
- // If we are called not at the first steps, this means we are beginning deceleration
- NVIC_SetPendingIRQ(TIMER2_IRQn);
- // Synchronize both counters
- LPC_TIM2->TC = LPC_TIM0->TC;
- }
-
- return 0;
-}
HCoop / clinton / Smoothieware.git / blobdiff