From cf5be5aade670b10b1a26a3c97dc41de81734aef Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Mon, 5 Jul 2021 18:20:50 +0200 Subject: [PATCH] PulseGen: initial version of the ramp/pulse generator --- src/config/todo.h | 9 +- src/modules/pulse_gen.cpp | 243 ++++++++++++++++++ src/modules/pulse_gen.h | 95 +++++++ tests/unit/modules/CMakeLists.txt | 1 + tests/unit/modules/pulse_gen/CMakeLists.txt | 16 ++ .../unit/modules/pulse_gen/test_pulse_gen.cpp | 35 +++ 6 files changed, 398 insertions(+), 1 deletion(-) create mode 100644 src/modules/pulse_gen.cpp create mode 100644 src/modules/pulse_gen.h create mode 100644 tests/unit/modules/pulse_gen/CMakeLists.txt create mode 100644 tests/unit/modules/pulse_gen/test_pulse_gen.cpp diff --git a/src/config/todo.h b/src/config/todo.h index 2dba1e1..3331837 100644 --- a/src/config/todo.h +++ b/src/config/todo.h @@ -4,4 +4,11 @@ #define F_CPU 16000000 #endif -#define MAX_STEP_FREQUENCY 40000 // Max step frequency +// Max step frequency 40KHz +#define MAX_STEP_FREQUENCY 40000 + +// Minimum stepper rate 120Hz. +#define MINIMAL_STEP_RATE 120 + +// Step frequency divider (influences the speed tables!) +#define STEP_TIMER_DIVIDER 8 diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp new file mode 100644 index 0000000..3196081 --- /dev/null +++ b/src/modules/pulse_gen.cpp @@ -0,0 +1,243 @@ +#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 { + +PulseGen::PulseGen() { + // Some initial values + position = 0; + acceleration = 1200; + block_buffer_head = block_buffer_tail = 0; + current_block = nullptr; + + // 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; +} + +// 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. + // 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) + + // Limit minimal step rate (Otherwise the timer will overflow.) + if (initial_rate < MINIMAL_STEP_RATE) + initial_rate = MINIMAL_STEP_RATE; + if (initial_rate > block->nominal_rate) + initial_rate = block->nominal_rate; + if (final_rate < MINIMAL_STEP_RATE) + final_rate = MINIMAL_STEP_RATE; + if (final_rate > block->nominal_rate) + final_rate = block->nominal_rate; + + rate_t acceleration = block->acceleration_st; + + // Don't allow zero acceleration. + if (acceleration == 0) + acceleration = 1; + + // estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration) + // (target_rate*target_rate-initial_rate*initial_rate)/(2.0*acceleration)); + rate_t initial_rate_sqr = initial_rate * initial_rate; + rate_t nominal_rate_sqr = block->nominal_rate * block->nominal_rate; + rate_t final_rate_sqr = final_rate * final_rate; + rate_t acceleration_x2 = acceleration << 1; + // ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, acceleration)); + steps_t accelerate_steps = (nominal_rate_sqr - initial_rate_sqr + acceleration_x2 - 1) / acceleration_x2; + // floor(estimate_acceleration_distance(block->nominal_rate, final_rate, -acceleration)); + steps_t decelerate_steps = (nominal_rate_sqr - final_rate_sqr) / acceleration_x2; + steps_t accel_decel_steps = accelerate_steps + decelerate_steps; + // Size of Plateau of Nominal Rate. + steps_t plateau_steps = 0; + + // 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; + } 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)); + // 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) + 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; + } 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) + 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; + } + } + + block->accelerate_until = accelerate_steps; + block->decelerate_after = accelerate_steps + plateau_steps; + block->initial_rate = initial_rate; + 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) { + // 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); + + // Bail if this is a zero-length block + if (block->step_event_count <= dropsegments) + return; + + // Compute direction bits for this block + block->direction = (target < position); + + 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 + block->acceleration = acceleration; + block->acceleration_rate = ((float)block->acceleration_st * (float)(16777216.0 / (F_CPU / 8.0))); + + // 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); + + // TODO: Move the buffer head + //block_buffer_head++; +} + +st_timer_t PulseGen::Step(const MotorParams &motorParams) { + if (!current_block) { + // TODO: fetch next block + if (!block_buffer_head) + current_block = &block_buffer[block_buffer_head++]; + 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); + step_events_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 (++step_events_completed >= current_block->step_event_count) + break; + } + + // Calculate new timer value + // 13.38-14.63us for steady state, + // 25.12us for acceleration / deceleration. + st_timer_t timer; + if (step_events_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 (step_events_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 (step_events_completed >= current_block->step_event_count) { + current_block = nullptr; + } + + return timer; +} + +} // namespace motor +} // namespace modules diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h new file mode 100644 index 0000000..93c0ef0 --- /dev/null +++ b/src/modules/pulse_gen.h @@ -0,0 +1,95 @@ +#pragma once +#include +#include "speed_table.h" +#include "../hal/tmc2130.h" + +namespace modules { +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; +}; + +class PulseGen { +public: + PulseGen(); + + float Acceleration() const { return acceleration; }; + void SetAcceleration(float accel) { acceleration = accel; } + + void Move(float x, float feed_rate); + float Position() const; + bool QueueEmpty() const; + bool Full() const; + + st_timer_t Step(const hal::tmc2130::MotorParams &motorParams); + +private: + //{ units constants + steps_t axis_steps_per_sqr_second; + float axis_steps_per_unit; + float max_jerk; + steps_t dropsegments; // segments are dropped if lower than that + //} + + //{ block buffer + 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; + + 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; + //} + + void calculate_trapezoid_for_block(block_t *block, float entry_speed, float exit_speed); +}; + +} // namespace pulse_gen +} // namespace modules diff --git a/tests/unit/modules/CMakeLists.txt b/tests/unit/modules/CMakeLists.txt index ae0026a..6467464 100644 --- a/tests/unit/modules/CMakeLists.txt +++ b/tests/unit/modules/CMakeLists.txt @@ -2,3 +2,4 @@ add_subdirectory(buttons) add_subdirectory(leds) add_subdirectory(protocol) add_subdirectory(speed_table) +add_subdirectory(pulse_gen) diff --git a/tests/unit/modules/pulse_gen/CMakeLists.txt b/tests/unit/modules/pulse_gen/CMakeLists.txt new file mode 100644 index 0000000..8f0bc24 --- /dev/null +++ b/tests/unit/modules/pulse_gen/CMakeLists.txt @@ -0,0 +1,16 @@ +# define the test executable +add_executable(pulse_gen_tests + test_pulse_gen.cpp + ../../../../src/modules/pulse_gen.cpp + ../../../../src/modules/speed_table.cpp + ../stubs/stub_shr16.cpp + ../stubs/stub_gpio.cpp +) + +# define required search paths +target_include_directories( + pulse_gen_tests PUBLIC ${CMAKE_SOURCE_DIR}/src/modules ${CMAKE_SOURCE_DIR}/src/hal +) + +# tell build system about the test case +add_catch_test(pulse_gen_tests) diff --git a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp new file mode 100644 index 0000000..84b66fd --- /dev/null +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -0,0 +1,35 @@ +#include "catch2/catch.hpp" +#include "pulse_gen.h" +#include "../pins.h" +#include + +using Catch::Matchers::Equals; +using namespace modules::pulse_gen; +using hal::tmc2130::MotorParams; + +TEST_CASE("pulse_gen::basic", "[pulse_gen]") { + MotorParams mp = { + .idx = 0, + .dirOn = config::idler.dirOn, + .csPin = IDLER_CS_PIN, + .stepPin = IDLER_STEP_PIN, + .sgPin = IDLER_SG_PIN, + .uSteps = config::idler.uSteps + }; + + for (int accel = 100; accel <= 5000; accel *= 2) { + PulseGen pg; + pg.SetAcceleration(accel); + pg.Move(100, 100); + + unsigned long ts = 0; + st_timer_t next; + do { + next = pg.Step(mp); + printf("%lu %u\n", ts, next); + ts += next; + } while (next); + + printf("\n\n"); + } +}