From 006dfd4abcdb231a2d63d7f03e906eec20348110 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 19:56:14 +0200 Subject: [PATCH] PulseGen: remove all floating point calculations Work in steps, steps/s, steps/s2 directly. --- src/modules/pulse_gen.cpp | 109 ++++++------------ src/modules/pulse_gen.h | 109 +++++++++--------- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 2 +- 3 files changed, 87 insertions(+), 133 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 3196081..500c5a8 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -18,24 +18,14 @@ PulseGen::PulseGen() { // TODO: configuration constants dropsegments = 5; - - // TODO: base units for the axis - steps_t max_acceleration_units_per_sq_second = 2500; // mm/s2 - axis_steps_per_unit = 100.f; // steps/mm - max_jerk = 10.f; - - // TODO: derived for trapezoid calculations - axis_steps_per_sqr_second = max_acceleration_units_per_sq_second * axis_steps_per_unit; + max_jerk = 100; } -// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors. -void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed) { - // These two lines are the only floating point calculations performed in this routine. - // initial_rate, final_rate in Hz. +void PulseGen::CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed) { // Minimum stepper rate 120Hz, maximum 40kHz. If the stepper rate goes above 10kHz, // the stepper interrupt routine groups the pulses by 2 or 4 pulses per interrupt tick. - rate_t initial_rate = ceil(entry_speed * block->speed_factor); // (step/min) - rate_t final_rate = ceil(exit_speed * block->speed_factor); // (step/min) + rate_t initial_rate = entry_speed; + rate_t final_rate = exit_speed; // Limit minimal step rate (Otherwise the timer will overflow.) if (initial_rate < MINIMAL_STEP_RATE) @@ -47,9 +37,8 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, if (final_rate > block->nominal_rate) final_rate = block->nominal_rate; - rate_t acceleration = block->acceleration_st; - // Don't allow zero acceleration. + rate_t acceleration = block->acceleration; if (acceleration == 0) acceleration = 1; @@ -70,39 +59,31 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will // have to use intersection_distance() to calculate when to abort acceleration and start braking // in order to reach the final_rate exactly at the end of this block. - if (accel_decel_steps < block->step_event_count) { - plateau_steps = block->step_event_count - accel_decel_steps; + if (accel_decel_steps < block->steps) { + plateau_steps = block->steps - accel_decel_steps; } else { uint32_t acceleration_x4 = acceleration << 2; // Avoid negative numbers if (final_rate_sqr >= initial_rate_sqr) { - // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->step_event_count)); + // accelerate_steps = ceil(intersection_distance(initial_rate, final_rate, acceleration, block->steps)); // intersection_distance(float initial_rate, float final_rate, float acceleration, float distance) // (2.0*acceleration*distance-initial_rate*initial_rate+final_rate*final_rate)/(4.0*acceleration); -#if 0 - accelerate_steps = (block->step_event_count >> 1) + (final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1 + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; -#else accelerate_steps = final_rate_sqr - initial_rate_sqr + acceleration_x4 - 1; - if (block->step_event_count & 1) + if (block->steps & 1) accelerate_steps += acceleration_x2; accelerate_steps /= acceleration_x4; - accelerate_steps += (block->step_event_count >> 1); -#endif - if (accelerate_steps > block->step_event_count) - accelerate_steps = block->step_event_count; + accelerate_steps += (block->steps >> 1); + if (accelerate_steps > block->steps) + accelerate_steps = block->steps; } else { -#if 0 - decelerate_steps = (block->step_event_count >> 1) + (initial_rate_sqr - final_rate_sqr + (block->step_event_count & 1) * acceleration_x2) / acceleration_x4; -#else decelerate_steps = initial_rate_sqr - final_rate_sqr; - if (block->step_event_count & 1) + if (block->steps & 1) decelerate_steps += acceleration_x2; decelerate_steps /= acceleration_x4; - decelerate_steps += (block->step_event_count >> 1); -#endif - if (decelerate_steps > block->step_event_count) - decelerate_steps = block->step_event_count; - accelerate_steps = block->step_event_count - decelerate_steps; + decelerate_steps += (block->steps >> 1); + if (decelerate_steps > block->steps) + decelerate_steps = block->steps; + accelerate_steps = block->steps - decelerate_steps; } } @@ -112,53 +93,31 @@ void PulseGen::calculate_trapezoid_for_block(block_t *block, float entry_speed, block->final_rate = final_rate; } -// Add a new linear movement to the buffer. steps_x, _y and _z is the absolute position in -// mm. Microseconds specify how many microseconds the move should take to perform. To aid acceleration -// calculation the caller must also provide the physical length of the line in millimeters. -void PulseGen::Move(float x, float feed_rate) { +void PulseGen::Move(pos_t target, steps_t feed_rate) { // Prepare to set up new block block_t *block = &block_buffer[block_buffer_head]; - // The target position of the tool in absolute steps - // Calculate target position in absolute steps - long target = lround(x * axis_steps_per_unit); - - block->step_event_count = abs(target - position); + block->steps = abs(target - position); // Bail if this is a zero-length block - if (block->step_event_count <= dropsegments) + if (block->steps <= dropsegments) return; - // Compute direction bits for this block - block->direction = (target < position); + // Direction and speed for this block + block->direction = (target > position); + block->nominal_rate = feed_rate; - float delta_mm = (target - position) / axis_steps_per_unit; - block->millimeters = abs(delta_mm); - - float inverse_millimeters = 1.0f / block->millimeters; // Inverse millimeters to remove multiple divides - - // Calculate speed in mm/second for each axis. No divide by zero due to previous checks. - float inverse_second = feed_rate * inverse_millimeters; - - block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0 - block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0 - - // Compute and limit the acceleration rate for the trapezoid generator. - float steps_per_mm = block->step_event_count / block->millimeters; - - // Acceleration of the segment, in mm/sec^2 - block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 + // Acceleration of the segment, in steps/sec^2 block->acceleration = acceleration; - block->acceleration_rate = ((float)block->acceleration_st * (float)(16777216.0 / (F_CPU / 8.0))); + block->acceleration_rate = block->acceleration * (rate_t)((float)F_CPU / (F_CPU / STEP_TIMER_DIVIDER)); - // Precalculate the division, so when all the trapezoids in the planner queue get recalculated, the division is not repeated. - block->speed_factor = block->nominal_rate / block->nominal_speed; - - // TODO: chain moves? - calculate_trapezoid_for_block(block, max_jerk, max_jerk); + // Perform the trapezoid calculations + CalculateTrapezoid(block, max_jerk, max_jerk); // TODO: Move the buffer head //block_buffer_head++; + + position = target; } st_timer_t PulseGen::Step(const MotorParams &motorParams) { @@ -176,7 +135,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { deceleration_time = 0; acc_step_rate = uint16_t(current_block->initial_rate); acceleration_time = calc_timer(acc_step_rate, step_loops); - step_events_completed = 0; + 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 @@ -187,7 +146,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // Step the motor for (uint8_t i = 0; i < step_loops; ++i) { TMC2130::Step(motorParams); - if (++step_events_completed >= current_block->step_event_count) + if (++steps_completed >= current_block->steps) break; } @@ -195,7 +154,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // 13.38-14.63us for steady state, // 25.12us for acceleration / deceleration. st_timer_t timer; - if (step_events_completed <= current_block->accelerate_until) { + 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); @@ -205,7 +164,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { // step_rate to timer interval timer = calc_timer(acc_step_rate, step_loops); acceleration_time += timer; - } else if (step_events_completed > current_block->decelerate_after) { + } 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 @@ -232,7 +191,7 @@ st_timer_t PulseGen::Step(const MotorParams &motorParams) { } // If current block is finished, reset pointer - if (step_events_completed >= current_block->step_event_count) { + if (steps_completed >= current_block->steps) { current_block = nullptr; } diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 93c0ef0..11149d1 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -4,91 +4,86 @@ #include "../hal/tmc2130.h" namespace modules { + +/// Acceleration ramp and stepper pulse generator namespace pulse_gen { using speed_table::st_timer_t; -typedef uint32_t steps_t; -typedef uint32_t rate_t; -typedef int32_t pos_t; - -struct block_t { - // Fields used by the bresenham algorithm for tracing the line - // steps_x.y,z, step_event_count, acceleration_rate, direction_bits and active_extruder are set by plan_buffer_line(). - steps_t step_event_count; // The number of step events required to complete this block - rate_t acceleration_rate; // The acceleration rate used for acceleration calculation - bool direction; // The direction bit set for this block (refers to *_DIRECTION_BIT in config.h) - - // accelerate_until and decelerate_after are set by calculate_trapezoid_for_block() and they need to be synchronized with the stepper interrupt controller. - steps_t accelerate_until; // The index of the step event on which to stop acceleration - steps_t decelerate_after; // The index of the step event on which to start decelerating - - // Fields used by the motion planner to manage acceleration - // float speed_x, speed_y, speed_z, speed_e; // Nominal mm/sec for each axis - // The nominal speed for this block in mm/sec. - // This speed may or may not be reached due to the jerk and acceleration limits. - float nominal_speed; - // Entry speed at previous-current junction in mm/sec, respecting the acceleration and jerk limits. - // The entry speed limit of the current block equals the exit speed of the preceding block. - //float entry_speed; - - // The total travel of this block in mm - float millimeters; - // acceleration mm/sec^2 - float acceleration; - - // Settings for the trapezoid generator (runs inside an interrupt handler). - // Changing the following values in the planner needs to be synchronized with the interrupt handler by disabling the interrupts. - rate_t nominal_rate; // The nominal step rate for this block in step_events/sec - rate_t initial_rate; // The jerk-adjusted step rate at start of block - rate_t final_rate; // The minimal rate at exit - rate_t acceleration_st; // acceleration steps/sec^2 - - // Pre-calculated division for the calculate_trapezoid_for_block() routine to run faster. - float speed_factor; -}; +typedef uint32_t steps_t; ///< Absolute step units +typedef uint32_t rate_t; ///< Type for step rates +typedef int32_t pos_t; ///< Axis position (signed) class PulseGen { public: PulseGen(); - float Acceleration() const { return acceleration; }; - void SetAcceleration(float accel) { acceleration = accel; } + /// @returns the acceleration for the axis + steps_t Acceleration() const { return acceleration; }; - void Move(float x, float feed_rate); - float Position() const; + /// Set acceleration for the axis + void SetAcceleration(steps_t accel) { acceleration = accel; } + + /// Plan a single move (can only be executed when !Full()) + void Move(pos_t x, steps_t feed_rate); + + /// @returns the current position of the axis + pos_t Position() const { return position; } + + /// Set the position of the axis + void SetPosition(pos_t x) { position = x; } + + /// @returns true if all planned moves have been finished bool QueueEmpty() const; + + /// @returns false if new moves can still be planned bool Full() const; + /// Single-step the axis + /// @returns the interval for the next tick st_timer_t Step(const hal::tmc2130::MotorParams &motorParams); private: + /// Motion parameters for the current planned or executing move + struct block_t { + steps_t steps; ///< Step events + bool direction; ///< The direction for this block + + rate_t acceleration_rate; ///< The acceleration rate + steps_t accelerate_until; ///< The index of the step event on which to stop acceleration + steps_t decelerate_after; ///< The index of the step event on which to start decelerating + + // Settings for the trapezoid generator (runs inside an interrupt handler) + rate_t nominal_rate; ///< The nominal step rate for this block in steps/sec + rate_t initial_rate; ///< Rate at start of block + rate_t final_rate; ///< Rate at exit + rate_t acceleration; ///< acceleration steps/sec^2 + }; + //{ units constants - steps_t axis_steps_per_sqr_second; - float axis_steps_per_unit; - float max_jerk; + steps_t max_jerk; steps_t dropsegments; // segments are dropped if lower than that //} - //{ block buffer + // Block buffer parameters block_t block_buffer[2]; block_t *current_block; uint8_t block_buffer_head; uint8_t block_buffer_tail; - //} - //{ state - pos_t position; - float acceleration; + // Axis data + pos_t position; ///< Current axis position + steps_t acceleration; ///< Current axis acceleration + // Step parameters rate_t acceleration_time, deceleration_time; st_timer_t acc_step_rate; // decelaration start point - uint8_t step_loops; - uint8_t step_loops_nominal; - st_timer_t timer_nominal; - steps_t step_events_completed; - //} + uint8_t step_loops; // steps per loop + uint8_t step_loops_nominal; // steps per loop at nominal speed + st_timer_t timer_nominal; // nominal interval + steps_t steps_completed; // steps completed - void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed); + /// Calculate the trapezoid parameters for the block + void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed); }; } // namespace pulse_gen diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp index 84b66fd..152f77c 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -20,7 +20,7 @@ TEST_CASE("pulse_gen::basic", "[pulse_gen]") { for (int accel = 100; accel <= 5000; accel *= 2) { PulseGen pg; pg.SetAcceleration(accel); - pg.Move(100, 100); + pg.Move(100000, 10000); unsigned long ts = 0; st_timer_t next;