From f6b32cbfef75ba3a1cbe5696e9de6169a9d69983 Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Tue, 7 Mar 2023 09:44:46 +0100 Subject: [PATCH] Prototype adaptive Idler homing sg_thrs Needs to be tweaked further to make it a production code. But my primary concern now is the fact that it doesn't seem to help too much while homing over the too-tightened Idler cover. --- src/hal/tmc2130.cpp | 16 ++++++- src/hal/tmc2130.h | 2 +- src/modules/idler.cpp | 33 +++++++++++++-- src/modules/idler.h | 7 +++- src/modules/motion.cpp | 4 +- src/modules/motion.h | 2 +- src/modules/movable_base.cpp | 8 ++++ src/modules/movable_base.h | 5 +++ src/modules/selector.cpp | 3 +- tests/unit/logic/homing/test_homing.cpp | 51 +++++++++++++++++++++++ tests/unit/logic/stubs/stub_motion.cpp | 9 ++-- tests/unit/logic/stubs/stub_motion.h | 1 + tests/unit/modules/stubs/stub_tmc2130.cpp | 19 +++++---- 13 files changed, 136 insertions(+), 24 deletions(-) diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp index bb32dac..bff75a1 100644 --- a/src/hal/tmc2130.cpp +++ b/src/hal/tmc2130.cpp @@ -261,8 +261,20 @@ void TMC2130::SetCurrents(const MotorParams ¶ms, const MotorCurrents ¤ } void TMC2130::SetSGTHRS(const MotorParams ¶ms) { - uint32_t tmc2130_coolConf = (((uint32_t)params.sg_thrs) << 16U); - WriteRegister(params, Registers::COOLCONF, tmc2130_coolConf); + union SGTHRSU { + struct __attribute__((packed)) S { + uint16_t zero; + uint16_t sgthrs; + constexpr S(uint16_t sgthrs) + : zero(0) + , sgthrs(sgthrs) {} + } s; + uint32_t dw; + constexpr SGTHRSU(uint16_t sgthrs) + : s(sgthrs) {} + }; + //uint32_t tmc2130_coolConf = (((uint32_t)params.sg_thrs) << 16U); + WriteRegister(params, Registers::COOLCONF, SGTHRSU(params.sg_thrs).dw); } void TMC2130::SetEnabled(const MotorParams ¶ms, bool enabled) { diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h index 5a95f7a..a4a5715 100644 --- a/src/hal/tmc2130.h +++ b/src/hal/tmc2130.h @@ -25,7 +25,7 @@ struct MotorParams { gpio::GPIO_pin stepPin; ///< step pin gpio::GPIO_pin sgPin; ///< StallGuard pin config::MRes mRes; ///< microstep resolution - int8_t sg_thrs; + int16_t sg_thrs; uint8_t axis; }; diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index eac5dbf..15382ea 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -20,12 +20,25 @@ void Idler::PrepareMoveToPlannedSlot() { } void Idler::PlanHomingMoveForward() { + // ml::leds.SetMode(0, ml::red, ml::off); + fwdHomeTrigger = true; + SetSGTHRS(32767); // @@TODO debug screw the homing sensitivity - to be removed for final + mm::motion.SetPosition(mm::Idler, mm::unitToSteps(config::IdlerOffsetFromHome)); + axisStart = mm::axisUnitToTruncatedUnit(mm::motion.CurPosition()); mm::motion.PlanMove(mm::unitToAxisUnit(config::idlerLimits.lenght * 2), mm::unitToAxisUnit(mg::globals.IdlerHomingFeedrate_deg_s())); dbg_logic_P(PSTR("Plan Homing Idler Forward")); } +void modules::idler::Idler::SetSGTHRS(int16_t sgthrs) { + mm::motion.PlanStallGuardThreshold(mm::Idler, sgthrs); + mm::motion.DriverForAxis(mm::Idler).SetSGTHRS(mm::axisParams[mm::Idler].params); +} + void Idler::PlanHomingMoveBack() { + fwdHomeTrigger = true; + SetSGTHRS(32767); //@@TODO debug screw + // ml::leds.SetMode(0, ml::red, ml::off); // we expect that we are at the front end of the axis, set the expected axis' position mm::motion.SetPosition(mm::Idler, mm::unitToSteps(config::idlerLimits.lenght)); axisStart = mm::axisUnitToTruncatedUnit(mm::motion.CurPosition()); @@ -36,8 +49,8 @@ void Idler::PlanHomingMoveBack() { bool Idler::FinishHomingAndPlanMoveToParkPos() { // check the axis' length - int32_t axisEnd = mm::axisUnitToTruncatedUnit(mm::motion.CurPosition()); - if (abs(axisEnd - axisStart) < (config::idlerLimits.lenght.v - 10)) { //@@TODO is 10 degrees ok? + if (AxisDistance(mm::axisUnitToTruncatedUnit(mm::motion.CurPosition())) + < (config::idlerLimits.lenght.v - 10)) { //@@TODO is 10 degrees ok? return false; // we couldn't home correctly, we cannot set the Idler's position } @@ -53,7 +66,7 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() { void Idler::FinishMove() { currentlyEngaged = plannedMove; - hal::tmc2130::MotorCurrents c = mm::motion.CurrentsForAxis(axis); + hal::tmc2130::MotorCurrents c = mm::motion.CurrentsForAxis(mm::Idler); if (Disengaged()) // reduce power into the Idler motor when disengaged (less force necessary) SetCurrents(c.iRun, c.iHold); else if (Engaged()) { // maximum motor power when the idler is engaged @@ -61,6 +74,20 @@ void Idler::FinishMove() { } } +void Idler::UpdateAdaptiveSGTHRS(bool forward) { + // return; + const uint8_t checkDistance = forward ? 220 : 200; + + if (AxisDistance(mm::axisUnitToTruncatedUnit(mm::motion.CurPosition())) + > checkDistance + && fwdHomeTrigger) { + // set higher sensitivity - i.e. lower threshold + // ml::leds.SetMode(0, ml::red, ml::on); + SetSGTHRS(mg::globals.StallGuardThreshold(mm::Idler) - 1); + fwdHomeTrigger = false; + } +} + Idler::OperationResult Idler::Disengage() { if (state == Moving || state == OnHold) { dbg_logic_P(PSTR("Moving --> Disengage refused")); diff --git a/src/modules/idler.h b/src/modules/idler.h index 18a8ec4..3c14cec 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -17,7 +17,8 @@ public: inline constexpr Idler() : MovableBase(mm::Idler) , plannedMove(Operation::disengage) - , currentlyEngaged(Operation::disengage) {} + , currentlyEngaged(Operation::disengage) + , fwdHomeTrigger(false) {} /// Plan engaging of the idler to a specific filament slot /// @param slot index to be activated @@ -70,6 +71,7 @@ protected: virtual void PlanHomingMoveBack() override; virtual bool FinishHomingAndPlanMoveToParkPos() override; virtual void FinishMove() override; + virtual void UpdateAdaptiveSGTHRS(bool forward) override; private: enum class Operation : uint8_t { @@ -85,6 +87,9 @@ private: /// current state Operation currentlyEngaged; + void SetSGTHRS(int16_t sgthrs); + + bool fwdHomeTrigger; }; /// The one and only instance of Idler in the FW diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index 72cba5a..7c3df1b 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -91,8 +91,8 @@ void Motion::StallGuardReset(Axis axis) { axisData[axis].drv.ClearStallguard(); } -void Motion::PlanStallGuardThreshold(config::Axis axis, uint8_t sg_thrs) { - axisParams[axis].params.sg_thrs = sg_thrs; +void Motion::PlanStallGuardThreshold(config::Axis axis, int16_t sg_thrs) { + mm::axisParams[axis].params.sg_thrs = sg_thrs; } void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) { diff --git a/src/modules/motion.h b/src/modules/motion.h index 235a198..491722b 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -105,7 +105,7 @@ public: /// Sets (plans) StallGuard threshold for an axis (basically the higher number the lower sensitivity) /// The new SGTHRS value gets applied in Init(), it is _NOT_ written into the TMC immediately in this method. - void PlanStallGuardThreshold(Axis axis, uint8_t sg_thrs); + void PlanStallGuardThreshold(Axis axis, int16_t sg_thrs); /// Enqueue a single axis move in steps starting and ending at zero speed with maximum /// feedrate. Moves can only be enqueued if the axis is not Full(). diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 1505221..cd67e97 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -74,6 +74,8 @@ void MovableBase::PerformHomeForward() { state = HomeBack; } else if (mm::motion.QueueEmpty(axis)) { HomeFailed(); + } else { + UpdateAdaptiveSGTHRS(true); } } @@ -92,6 +94,8 @@ void MovableBase::PerformHomeBack() { } } else if (mm::motion.QueueEmpty(axis)) { HomeFailed(); + } else { + UpdateAdaptiveSGTHRS(false); } } @@ -112,5 +116,9 @@ void MovableBase::CheckTMC() { } } +uint16_t __attribute__((noinline)) MovableBase::AxisDistance(int32_t curPos) const { + return abs(curPos - axisStart); +} + } // namespace motion } // namespace modules diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index c32aae7..23c337f 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -110,6 +110,9 @@ protected: virtual bool FinishHomingAndPlanMoveToParkPos() = 0; virtual void FinishMove() = 0; + /// default implementation is empty + virtual void UpdateAdaptiveSGTHRS(bool /*forward*/) {} + /// Initializes movement of a movable module. /// Beware: this operation reinitializes the axis/TMC driver as well (may introduce axis creep as we have seen on the Idler) OperationResult InitMovement(); @@ -125,6 +128,8 @@ protected: void HomeFailed(); void CheckTMC(); + + uint16_t AxisDistance(int32_t curPos) const; }; } // namespace motion diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index d645ddf..64410a0 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -35,8 +35,7 @@ void Selector::PlanHomingMoveBack() { bool Selector::FinishHomingAndPlanMoveToParkPos() { // check the axis' length - int32_t axisEnd = mm::axisUnitToTruncatedUnit(mm::motion.CurPosition()); - if (abs(axisEnd - axisStart) < (config::selectorLimits.lenght.v - 3)) { //@@TODO is 3mm ok? + if (AxisDistance(mm::axisUnitToTruncatedUnit(mm::motion.CurPosition())) < (config::selectorLimits.lenght.v - 3)) { //@@TODO is 3mm ok? return false; // we couldn't home correctly, we cannot set the Selector's position } diff --git a/tests/unit/logic/homing/test_homing.cpp b/tests/unit/logic/homing/test_homing.cpp index 9de4a33..ecce700 100644 --- a/tests/unit/logic/homing/test_homing.cpp +++ b/tests/unit/logic/homing/test_homing.cpp @@ -14,6 +14,7 @@ #include "../../../../src/logic/home.h" #include "../../../../src/logic/load_filament.h" #include "../../../../src/logic/unload_filament.h" +#include "../../../../src/logic/no_command.h" #include "../../modules/stubs/stub_adc.h" @@ -195,3 +196,53 @@ TEST_CASE("homing::on-hold", "[homing]") { REQUIRE(OnHold(slot)); } } + +void AdaptiveIdlerHoming() { + // prepare startup conditions + ForceReinitAllAutomata(); + + mi::idler.InvalidateHoming(); + + // idler should plan the homing move, position of the Idler should be 0 + main_loop(); + CHECK(mm::motion.CurPosition().v == mm::unitToSteps(config::IdlerOffsetFromHome) + 1); // magic constant just to tune the motor steps + CHECK(mi::idler.axisStart == config::IdlerOffsetFromHome.v + 2); + CHECK(mm::axes[mm::Idler].sg_thrs == 32767); + // do exact number of steps before triggering SG + uint32_t idlerSteps = mm::unitToSteps(config::idlerLimits.lenght); + uint32_t sgChange = mm::unitToAxisUnit(config::idlerLimits.lenght - 15.0_deg).v; + for (uint32_t i = 0; i < sgChange; ++i) { + main_loop(); + } + CHECK(mm::axes[mm::Idler].sg_thrs <= config::idler.sg_thrs); + + // finish the forward homing move to the correct distance + for (uint32_t i = sgChange; i < idlerSteps; ++i) { + main_loop(); + } + + mm::TriggerStallGuard(mm::Idler); + main_loop(); + mm::motion.StallGuardReset(mm::Idler); + + // now do a correct amount of steps of each axis towards the other end + uint32_t maxSteps = idlerSteps + 1; + + for (uint32_t i = 0; i < maxSteps; ++i) { + main_loop(); + if (i == idlerSteps) { + mm::TriggerStallGuard(mm::Idler); + } else { + mm::motion.StallGuardReset(mm::Idler); + } + } + + // now the Idler shall perform a move into their parking positions + while (mi::idler.State() != mm::MovableBase::Ready) { + main_loop(); + } +} + +TEST_CASE("homing::adaptive", "[homing]") { + AdaptiveIdlerHoming(); +} diff --git a/tests/unit/logic/stubs/stub_motion.cpp b/tests/unit/logic/stubs/stub_motion.cpp index 26ec7ff..d458157 100644 --- a/tests/unit/logic/stubs/stub_motion.cpp +++ b/tests/unit/logic/stubs/stub_motion.cpp @@ -10,9 +10,9 @@ Motion motion; // Intentionally inited with strange values // Need to call ReinitMotion() each time we start some unit test AxisSim axes[3] = { - { -32767, false, false, false, {} }, // pulley - { -32767, false, false, false, {} }, // selector //@@TODO proper selector positions once defined - { -32767, false, false, false, {} }, // idler + { -32767, false, false, false, config::pulley.sg_thrs, {} }, // pulley + { -32767, false, false, false, config::selector.sg_thrs, {} }, // selector //@@TODO proper selector positions once defined + { -32767, false, false, false, config::idler.sg_thrs, {} }, // idler }; bool Motion::InitAxis(Axis axis) { @@ -41,8 +41,9 @@ void TriggerStallGuard(Axis axis) { axes[axis].stallGuard = true; } -void Motion::PlanStallGuardThreshold(Axis axis, uint8_t sg_thrs) { +void Motion::PlanStallGuardThreshold(Axis axis, int16_t sg_thrs) { // do nothing for now + axes[axis].sg_thrs = sg_thrs; } void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) { diff --git a/tests/unit/logic/stubs/stub_motion.h b/tests/unit/logic/stubs/stub_motion.h index f68bc7f..2b7c302 100644 --- a/tests/unit/logic/stubs/stub_motion.h +++ b/tests/unit/logic/stubs/stub_motion.h @@ -10,6 +10,7 @@ struct AxisSim { bool enabled; bool homed; bool stallGuard; + int16_t sg_thrs; std::deque plannedMoves; }; diff --git a/tests/unit/modules/stubs/stub_tmc2130.cpp b/tests/unit/modules/stubs/stub_tmc2130.cpp index e769263..c7e0ac6 100644 --- a/tests/unit/modules/stubs/stub_tmc2130.cpp +++ b/tests/unit/modules/stubs/stub_tmc2130.cpp @@ -3,30 +3,33 @@ namespace hal { namespace tmc2130 { -void TMC2130::SetMode(const MotorParams ¶ms, MotorMode mode) { +void TMC2130::SetMode(const MotorParams & /*params*/, MotorMode /*mode*/) { // TODO } -bool TMC2130::Init(const MotorParams ¶ms, - const MotorCurrents ¤ts, - MotorMode mode) { +bool TMC2130::Init(const MotorParams & /*params*/, + const MotorCurrents & /*currents*/, + MotorMode /*mode*/) { // TODO return true; } -void TMC2130::SetEnabled(const MotorParams ¶ms, bool enabled) { +void TMC2130::SetEnabled(const MotorParams & /*params*/, bool enabled) { this->enabled = enabled; } -void TMC2130::SetCurrents(const MotorParams ¶ms, const MotorCurrents ¤ts) { +void TMC2130::SetCurrents(const MotorParams & /*params*/, const MotorCurrents & /*currents*/) { } -void TMC2130::Isr(const MotorParams ¶ms) { +void TMC2130::Isr(const MotorParams & /*params*/) { } -bool TMC2130::CheckForErrors(const MotorParams ¶ms) { +bool TMC2130::CheckForErrors(const MotorParams & /*params*/) { return !errorFlags.Good(); } +void TMC2130::SetSGTHRS(const MotorParams & /*params*/) { +} + } // namespace tmc2130 } // namespace hal