PulseGen: move Step() to header for inlining
Yuri D'Elia
parent
6f518f1ed2
commit
44a263d334
|
|
@ -1,10 +1,4 @@
|
|||
#include "pulse_gen.h"
|
||||
using hal::tmc2130::MotorParams;
|
||||
using hal::tmc2130::TMC2130;
|
||||
using modules::math::mulU24X24toH16;
|
||||
using modules::speed_table::calc_timer;
|
||||
|
||||
#include "../cmath.h"
|
||||
|
||||
namespace modules {
|
||||
namespace pulse_gen {
|
||||
|
|
@ -117,84 +111,5 @@ void PulseGen::Move(pos_t target, steps_t feed_rate) {
|
|||
position = target;
|
||||
}
|
||||
|
||||
st_timer_t PulseGen::Step(const MotorParams &motorParams) {
|
||||
if (!current_block) {
|
||||
// fetch next block
|
||||
if (!block_index.empty())
|
||||
current_block = &block_buffer[block_index.front()];
|
||||
if (!current_block)
|
||||
return 0;
|
||||
|
||||
// Set direction early so that the direction-change delay is accounted for
|
||||
TMC2130::SetDir(motorParams, current_block->direction);
|
||||
|
||||
// Initializes the trapezoid generator from the current block.
|
||||
deceleration_time = 0;
|
||||
acc_step_rate = uint16_t(current_block->initial_rate);
|
||||
acceleration_time = calc_timer(acc_step_rate, step_loops);
|
||||
steps_completed = 0;
|
||||
|
||||
// Set the nominal step loops to zero to indicate, that the timer value is not
|
||||
// known yet. That means, delay the initialization of nominal step rate and step
|
||||
// loops until the steady state is reached.
|
||||
step_loops_nominal = 0;
|
||||
}
|
||||
|
||||
// Step the motor
|
||||
for (uint8_t i = 0; i < step_loops; ++i) {
|
||||
TMC2130::Step(motorParams);
|
||||
if (++steps_completed >= current_block->steps)
|
||||
break;
|
||||
}
|
||||
|
||||
// Calculate new timer value
|
||||
// 13.38-14.63us for steady state,
|
||||
// 25.12us for acceleration / deceleration.
|
||||
st_timer_t timer;
|
||||
if (steps_completed <= current_block->accelerate_until) {
|
||||
// v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
|
||||
acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate);
|
||||
acc_step_rate += uint16_t(current_block->initial_rate);
|
||||
// upper limit
|
||||
if (acc_step_rate > uint16_t(current_block->nominal_rate))
|
||||
acc_step_rate = current_block->nominal_rate;
|
||||
// step_rate to timer interval
|
||||
timer = calc_timer(acc_step_rate, step_loops);
|
||||
acceleration_time += timer;
|
||||
} else if (steps_completed > current_block->decelerate_after) {
|
||||
st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate);
|
||||
|
||||
if (step_rate > acc_step_rate) { // Check step_rate stays positive
|
||||
step_rate = uint16_t(current_block->final_rate);
|
||||
} else {
|
||||
step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
|
||||
|
||||
// lower limit
|
||||
if (step_rate < current_block->final_rate)
|
||||
step_rate = uint16_t(current_block->final_rate);
|
||||
}
|
||||
|
||||
// Step_rate to timer interval.
|
||||
timer = calc_timer(step_rate, step_loops);
|
||||
deceleration_time += timer;
|
||||
} else {
|
||||
if (!step_loops_nominal) {
|
||||
// Calculation of the steady state timer rate has been delayed to the 1st tick
|
||||
// of the steady state to lower the initial interrupt blocking.
|
||||
timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
|
||||
step_loops_nominal = step_loops;
|
||||
}
|
||||
timer = timer_nominal;
|
||||
}
|
||||
|
||||
// If current block is finished, reset pointer
|
||||
if (steps_completed >= current_block->steps) {
|
||||
current_block = nullptr;
|
||||
block_index.pop();
|
||||
}
|
||||
|
||||
return timer;
|
||||
}
|
||||
|
||||
} // namespace motor
|
||||
} // namespace modules
|
||||
|
|
|
|||
|
|
@ -3,6 +3,7 @@
|
|||
#include "speed_table.h"
|
||||
#include "../hal/tmc2130.h"
|
||||
#include "../hal/circular_buffer.h"
|
||||
#include "../cmath.h"
|
||||
|
||||
namespace modules {
|
||||
|
||||
|
|
@ -10,6 +11,9 @@ namespace modules {
|
|||
namespace pulse_gen {
|
||||
|
||||
using config::blockBufferSize;
|
||||
using hal::tmc2130::TMC2130;
|
||||
using math::mulU24X24toH16;
|
||||
using speed_table::calc_timer;
|
||||
using speed_table::st_timer_t;
|
||||
typedef uint32_t steps_t; ///< Absolute step units
|
||||
typedef uint32_t rate_t; ///< Type for step rates
|
||||
|
|
@ -42,7 +46,84 @@ public:
|
|||
|
||||
/// Single-step the axis
|
||||
/// @returns the interval for the next tick
|
||||
st_timer_t Step(const hal::tmc2130::MotorParams &motorParams);
|
||||
inline st_timer_t Step(const hal::tmc2130::MotorParams &motorParams) {
|
||||
if (!current_block) {
|
||||
// fetch next block
|
||||
if (!block_index.empty())
|
||||
current_block = &block_buffer[block_index.front()];
|
||||
if (!current_block)
|
||||
return 0;
|
||||
|
||||
// Set direction early so that the direction-change delay is accounted for
|
||||
TMC2130::SetDir(motorParams, current_block->direction);
|
||||
|
||||
// Initializes the trapezoid generator from the current block.
|
||||
deceleration_time = 0;
|
||||
acc_step_rate = uint16_t(current_block->initial_rate);
|
||||
acceleration_time = calc_timer(acc_step_rate, step_loops);
|
||||
steps_completed = 0;
|
||||
|
||||
// Set the nominal step loops to zero to indicate, that the timer value is not
|
||||
// known yet. That means, delay the initialization of nominal step rate and step
|
||||
// loops until the steady state is reached.
|
||||
step_loops_nominal = 0;
|
||||
}
|
||||
|
||||
// Step the motor
|
||||
for (uint8_t i = 0; i < step_loops; ++i) {
|
||||
TMC2130::Step(motorParams);
|
||||
if (++steps_completed >= current_block->steps)
|
||||
break;
|
||||
}
|
||||
|
||||
// Calculate new timer value
|
||||
// 13.38-14.63us for steady state,
|
||||
// 25.12us for acceleration / deceleration.
|
||||
st_timer_t timer;
|
||||
if (steps_completed <= current_block->accelerate_until) {
|
||||
// v = t * a -> acc_step_rate = acceleration_time * current_block->acceleration_rate
|
||||
acc_step_rate = mulU24X24toH16(acceleration_time, current_block->acceleration_rate);
|
||||
acc_step_rate += uint16_t(current_block->initial_rate);
|
||||
// upper limit
|
||||
if (acc_step_rate > uint16_t(current_block->nominal_rate))
|
||||
acc_step_rate = current_block->nominal_rate;
|
||||
// step_rate to timer interval
|
||||
timer = calc_timer(acc_step_rate, step_loops);
|
||||
acceleration_time += timer;
|
||||
} else if (steps_completed > current_block->decelerate_after) {
|
||||
st_timer_t step_rate = mulU24X24toH16(deceleration_time, current_block->acceleration_rate);
|
||||
|
||||
if (step_rate > acc_step_rate) { // Check step_rate stays positive
|
||||
step_rate = uint16_t(current_block->final_rate);
|
||||
} else {
|
||||
step_rate = acc_step_rate - step_rate; // Decelerate from acceleration end point.
|
||||
|
||||
// lower limit
|
||||
if (step_rate < current_block->final_rate)
|
||||
step_rate = uint16_t(current_block->final_rate);
|
||||
}
|
||||
|
||||
// Step_rate to timer interval.
|
||||
timer = calc_timer(step_rate, step_loops);
|
||||
deceleration_time += timer;
|
||||
} else {
|
||||
if (!step_loops_nominal) {
|
||||
// Calculation of the steady state timer rate has been delayed to the 1st tick
|
||||
// of the steady state to lower the initial interrupt blocking.
|
||||
timer_nominal = calc_timer(uint16_t(current_block->nominal_rate), step_loops);
|
||||
step_loops_nominal = step_loops;
|
||||
}
|
||||
timer = timer_nominal;
|
||||
}
|
||||
|
||||
// If current block is finished, reset pointer
|
||||
if (steps_completed >= current_block->steps) {
|
||||
current_block = nullptr;
|
||||
block_index.pop();
|
||||
}
|
||||
|
||||
return timer;
|
||||
}
|
||||
|
||||
private:
|
||||
/// Motion parameters for the current planned or executing move
|
||||
|
|
|
|||
Loading…
Reference in New Issue