From 16e7f62aee79934b0dc27a1ea5dfce99c3cf41da Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Tue, 13 Jul 2021 00:48:44 +0200 Subject: [PATCH] PulseGen: introduce CurPosition() for testing CurPosition() returns the live axis position, which in this implementation is inherently expensive to compute. This shouldn't be required for the MMU, but it /will/ come in handy to check for the axis position/s in Motion tests. --- src/modules/pulse_gen.cpp | 38 +++++++++++------ src/modules/pulse_gen.h | 25 +++++++++-- .../unit/modules/pulse_gen/test_pulse_gen.cpp | 42 ++++++++++++++++++- 3 files changed, 89 insertions(+), 16 deletions(-) diff --git a/src/modules/pulse_gen.cpp b/src/modules/pulse_gen.cpp index 0c09573..b8e51be 100644 --- a/src/modules/pulse_gen.cpp +++ b/src/modules/pulse_gen.cpp @@ -115,20 +115,34 @@ bool PulseGen::PlanMoveTo(pos_t target, steps_t feed_rate) { return true; } +pos_t PulseGen::CurPosition() const { + pos_t cur_pos = position; + circular_index_t iter = block_index; + + // if we have a live block remove the partial offset + if (current_block) { + cur_pos -= CurBlockShift(); + iter.pop(); + } + + // rollback remaining blocks + while (!iter.empty()) { + cur_pos -= BlockShift(&block_buffer[iter.front()]); + iter.pop(); + } + + return cur_pos; +} + void PulseGen::AbortPlannedMoves() { - if (!current_block) - return; + // always update to effective position + position = CurPosition(); - // update position - steps_t steps_missing = (current_block->steps - steps_completed); - if (current_block->direction) - position -= steps_missing; - else - position += steps_missing; - - // destroy the block - current_block = nullptr; - block_index.pop(); + // destroy the current block + if (current_block) { + current_block = nullptr; + block_index.pop(); + } } } // namespace motor diff --git a/src/modules/pulse_gen.h b/src/modules/pulse_gen.h index 7c6edb5..b3ca9a5 100644 --- a/src/modules/pulse_gen.h +++ b/src/modules/pulse_gen.h @@ -14,6 +14,7 @@ using hal::tmc2130::TMC2130; using math::mulU24X24toH16; using speed_table::calc_timer; using speed_table::st_timer_t; +typedef CircularIndex circular_index_t; 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) @@ -35,13 +36,20 @@ public: /// @returns true if the move has been enqueued bool PlanMoveTo(pos_t pos, steps_t feedrate); - /// stop whatever moves are being done + /// Stop whatever moves are being done void AbortPlannedMoves(); - /// @returns the current position of the axis + /// @returns the position of the axis at the end of all moves pos_t Position() const { return position; } + /// Fetch the current position of the axis while stepping. This function is expensive! + /// It's necessary only in exceptional cases. For regular usage, Position() should + /// probably be used instead. + /// @returns the current position of the axis + pos_t CurPosition() const; + /// Set the position of the axis + /// Should only be called when the queue is empty. void SetPosition(pos_t x) { position = x; } /// @returns true if all planned moves have been finished @@ -150,7 +158,7 @@ private: // Block buffer parameters block_t block_buffer[blockBufferSize]; - CircularIndex block_index; + circular_index_t block_index; block_t *current_block; // Axis data @@ -168,6 +176,17 @@ private: /// Calculate the trapezoid parameters for the block void CalculateTrapezoid(block_t *block, steps_t entry_speed, steps_t exit_speed); + + /// Return the axis shift introduced by the current (live) block + inline pos_t CurBlockShift() const { + steps_t steps_missing = (current_block->steps - steps_completed); + return current_block->direction ? steps_missing : -steps_missing; + } + + /// Return the axis shift introduced by the specified full block + static inline pos_t BlockShift(const block_t *block) { + return block->direction ? block->steps : -block->steps; + } }; } // 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 3a2b105..e62a9fb 100644 --- a/tests/unit/modules/pulse_gen/test_pulse_gen.cpp +++ b/tests/unit/modules/pulse_gen/test_pulse_gen.cpp @@ -112,10 +112,14 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") { pg.PlanMoveTo(10, 10); bool st = ReadPin(IDLER_STEP_PIN) == Level::high; for (size_t i = 0; i != 10; ++i) { + // check current axis position + REQUIRE((pos_t)i == pg.CurPosition()); + + // perform the step REQUIRE(pg.Step(mp) > 0); bool newSt = ReadPin(IDLER_STEP_PIN) == Level::high; - // Assuming DEDGE each step should toggle the pin + // assuming DEDGE each step should toggle the pin REQUIRE(newSt != st); st = newSt; } @@ -129,6 +133,42 @@ TEST_CASE("pulse_gen::step_count", "[pulse_gen]") { REQUIRE(pg.Position() == 10); } +TEST_CASE("pulse_gen::queue_position", "[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 + }; + + PulseGen pg(10, 100); + + // enqueue two moves, observing Position and CurPosition. + REQUIRE(pg.Position() == 0); + REQUIRE(pg.CurPosition() == 0); + + // while enqueuing Position should move but CurPosition should not + pg.PlanMoveTo(10, 10); + REQUIRE(pg.Position() == 10); + REQUIRE(pg.CurPosition() == 0); + + pg.PlanMoveTo(15, 10); + REQUIRE(pg.Position() == 15); + REQUIRE(pg.CurPosition() == 0); + + // step through the moves manually, cycling through two blocks + for (size_t i = 0; i != 15; ++i) { + REQUIRE((pos_t)i == pg.CurPosition()); + REQUIRE(pg.Position() == 15); + pg.Step(mp); + } + + // the final positions should match + REQUIRE(pg.CurPosition() == pg.Position()); +} + TEST_CASE("pulse_gen::queue_size", "[pulse_gen]") { PulseGen pg(10, 100);