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

@@ -261,8 +261,20 @@ void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &curren
 }
 
 void TMC2130::SetSGTHRS(const MotorParams &params) {
-    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 &params, bool enabled) {

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;
 };
 

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<mm::I_pos_t>(config::IdlerOffsetFromHome));
+    axisStart = mm::axisUnitToTruncatedUnit<config::U_deg>(mm::motion.CurPosition<mm::Idler>());
     mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(config::idlerLimits.lenght * 2),
         mm::unitToAxisUnit<mm::I_speed_t>(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<mm::I_pos_t>(config::idlerLimits.lenght));
     axisStart = mm::axisUnitToTruncatedUnit<config::U_deg>(mm::motion.CurPosition<mm::Idler>());
@@ -36,8 +49,8 @@ void Idler::PlanHomingMoveBack() {
 
 bool Idler::FinishHomingAndPlanMoveToParkPos() {
     // check the axis' length
-    int32_t axisEnd = mm::axisUnitToTruncatedUnit<config::U_deg>(mm::motion.CurPosition<mm::Idler>());
-    if (abs(axisEnd - axisStart) < (config::idlerLimits.lenght.v - 10)) { //@@TODO is 10 degrees ok?
+    if (AxisDistance(mm::axisUnitToTruncatedUnit<config::U_deg>(mm::motion.CurPosition<mm::Idler>()))
+        < (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<config::U_deg>(mm::motion.CurPosition<mm::Idler>()))
+            > 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"));

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

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) {

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().

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

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

src/modules/selector.cpp

@@ -35,8 +35,7 @@ void Selector::PlanHomingMoveBack() {
 
 bool Selector::FinishHomingAndPlanMoveToParkPos() {
     // check the axis' length
-    int32_t axisEnd = mm::axisUnitToTruncatedUnit<config::U_mm>(mm::motion.CurPosition<mm::Selector>());
-    if (abs(axisEnd - axisStart) < (config::selectorLimits.lenght.v - 3)) { //@@TODO is 3mm ok?
+    if (AxisDistance(mm::axisUnitToTruncatedUnit<config::U_mm>(mm::motion.CurPosition<mm::Selector>())) < (config::selectorLimits.lenght.v - 3)) { //@@TODO is 3mm ok?
         return false; // we couldn't home correctly, we cannot set the Selector's position
     }
 

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<mm::Idler>().v == mm::unitToSteps<mm::I_pos_t>(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<mm::I_pos_t>(config::idlerLimits.lenght);
+    uint32_t sgChange = mm::unitToAxisUnit<mm::I_pos_t>(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();
+}

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) {

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<pos_t> plannedMoves;
 };
 

tests/unit/modules/stubs/stub_tmc2130.cpp

@@ -3,30 +3,33 @@
 namespace hal {
 namespace tmc2130 {
 
-void TMC2130::SetMode(const MotorParams &params, MotorMode mode) {
+void TMC2130::SetMode(const MotorParams & /*params*/, MotorMode /*mode*/) {
     // TODO
 }
 
-bool TMC2130::Init(const MotorParams &params,
-    const MotorCurrents &currents,
-    MotorMode mode) {
+bool TMC2130::Init(const MotorParams & /*params*/,
+    const MotorCurrents & /*currents*/,
+    MotorMode /*mode*/) {
     // TODO
     return true;
 }
 
-void TMC2130::SetEnabled(const MotorParams &params, bool enabled) {
+void TMC2130::SetEnabled(const MotorParams & /*params*/, bool enabled) {
     this->enabled = enabled;
 }
 
-void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &currents) {
+void TMC2130::SetCurrents(const MotorParams & /*params*/, const MotorCurrents & /*currents*/) {
 }
 
-void TMC2130::Isr(const MotorParams &params) {
+void TMC2130::Isr(const MotorParams & /*params*/) {
 }
 
-bool TMC2130::CheckForErrors(const MotorParams &params) {
+bool TMC2130::CheckForErrors(const MotorParams & /*params*/) {
     return !errorFlags.Good();
 }
 
+void TMC2130::SetSGTHRS(const MotorParams & /*params*/) {
+}
+
 } // namespace tmc2130
 } // namespace hal