WIP: homing on both ends of axes

The principle has been implemented, but the TMC is not providing
the right data for some reason - homing doesn't work at all right now.

Also, after solving the physical homing, unit tests must be updated.

src/modules/idler.cpp

@@ -17,12 +17,26 @@ void Idler::PrepareMoveToPlannedSlot() {
     dbg_logic_fP(PSTR("Prepare Move Idler slot %d"), plannedSlot);
 }
 
-void Idler::PlanHomingMove() {
-    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(-config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
-    dbg_logic_P(PSTR("Plan Homing Idler"));
+void Idler::PlanHomingMoveForward() {
+    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
+    dbg_logic_P(PSTR("Plan Homing Idler Forward"));
 }
 
-void Idler::FinishHomingAndPlanMoveToParkPos() {
+void Idler::PlanHomingMoveBack() {
+    // 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::stepsToUnit<mm::I_pos_t>(mm::I_pos_t({ 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>(config::idlerFeedrate));
+    dbg_logic_P(PSTR("Plan Homing Idler Back"));
+}
+
+bool Idler::FinishHomingAndPlanMoveToParkPos() {
+    // check the axis' length
+    int32_t axisEnd = mm::stepsToUnit<mm::I_pos_t>(mm::I_pos_t({ mm::motion.CurPosition(mm::Idler) }));
+    if (abs(axisEnd - axisStart) < (config::idlerLimits.lenght.v - 10)) { //@@TODO is 10 degrees ok?
+        return false; // we couldn't home correctly, we cannot set the Idler's position
+    }
+
     mm::motion.SetPosition(mm::Idler, 0);
 
     // finish whatever has been planned before homing
@@ -30,6 +44,7 @@ void Idler::FinishHomingAndPlanMoveToParkPos() {
         plannedSlot = IdleSlotIndex();
     }
     InitMovement();
+    return true;
 }
 
 void Idler::FinishMove() {
@@ -48,7 +63,7 @@ Idler::OperationResult Idler::Disengage() {
 
     // coordinates invalid, first home, then disengage
     if (!homingValid) {
-        PerformHome();
+        PerformHomeForward();
         return OperationResult::Accepted;
     }
 
@@ -72,7 +87,7 @@ Idler::OperationResult Idler::Engage(uint8_t slot) {
     plannedEngage = true;
 
     // if we are homing right now, just record the desired planned slot and return Accepted
-    if (state == Homing) {
+    if (state == HomeBack) {
         return OperationResult::Accepted;
     }
 
@@ -101,9 +116,16 @@ bool Idler::Step() {
         // dbg_logic_P(PSTR("Moving Idler"));
         PerformMove();
         return false;
-    case Homing:
-        dbg_logic_P(PSTR("Homing Idler"));
-        PerformHome();
+    case HomeForward:
+        dbg_logic_P(PSTR("Homing Idler Forward"));
+        PerformHomeForward();
+        return false;
+    case HomeMoveAwayFromForward:
+        PerformMoveAwayFromForward();
+        return false;
+    case HomeBack:
+        dbg_logic_P(PSTR("Homing Idler Back"));
+        PerformHomeBack();
         return false;
     case Ready:
         if (!homingValid && mg::globals.FilamentLoaded() < mg::InFSensor) {

src/modules/idler.h

@@ -53,8 +53,9 @@ public:
 
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
-    virtual void PlanHomingMove() override;
-    virtual void FinishHomingAndPlanMoveToParkPos() override;
+    virtual void PlanHomingMoveForward() override;
+    virtual void PlanHomingMoveBack() override;
+    virtual bool FinishHomingAndPlanMoveToParkPos() override;
     virtual void FinishMove() override;
 
 private:

src/modules/movable_base.cpp

@@ -13,8 +13,8 @@ void MovableBase::PlanHome() {
     mm::motion.StallGuardReset(axis);
 
     // plan move at least as long as the axis can go from one side to the other
-    PlanHomingMove(); // mm::motion.PlanMove(axis, delta, 1000);
-    state = Homing;
+    PlanHomingMoveForward(); // mm::motion.PlanMove(axis, delta, 1000);
+    state = HomeForward;
 }
 
 MovableBase::OperationResult MovableBase::InitMovement() {
@@ -41,21 +41,52 @@ void MovableBase::PerformMove() {
     }
 }
 
-void MovableBase::PerformHome() {
+void MovableBase::PerformHomeForward() {
     if (mm::motion.StallGuard(axis)) {
-        // we have reached the end of the axis - homed ok
+        // we have reached the front end of the axis - first part homed probably ok
+        mm::motion.StallGuardReset(axis);
+        mm::motion.AbortPlannedMoves(axis, true);
+        PlanHomingMoveBack();
+        state = HomeMoveAwayFromForward;
+    } else if (mm::motion.QueueEmpty(axis)) {
+        HomeFailed();
+    }
+}
+
+void MovableBase::PerformMoveAwayFromForward() {
+    // need to wait for the TMC to report "no-stall", otherwise we may get stuck in the forward stalled position forever
+    if (!mm::motion.StallGuard(axis)) {
+        mm::motion.StallGuardReset(axis);
+        state = HomeBack;
+    } else if (mm::motion.QueueEmpty(axis)) {
+        HomeFailed();
+    }
+}
+
+void MovableBase::PerformHomeBack() {
+    if (mm::motion.StallGuard(axis)) {
+        // we have reached the back end of the axis - second part homed probably ok
+        mm::motion.StallGuardReset(axis);
         mm::motion.AbortPlannedMoves(axis, true);
         mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal);
-        homingValid = true;
-        FinishHomingAndPlanMoveToParkPos();
-        // state = Ready; // not yet - we have to move to our parking position after homing the axis
+        if (!FinishHomingAndPlanMoveToParkPos()) {
+            // the measured axis' length was incorrect, something is blocking it, report an error, homing procedure terminated
+            state = HomingFailed;
+        } else {
+            homingValid = true;
+            // state = Ready; // not yet - we have to move to our parking position after homing the axis
+        }
     } else if (mm::motion.QueueEmpty(axis)) {
-        // we ran out of planned moves but no StallGuard event has occurred - homing failed
-        homingValid = false;
-        mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal);
-        state = HomingFailed;
+        HomeFailed();
     }
 }
 
+void MovableBase::HomeFailed() {
+    // we ran out of planned moves but no StallGuard event has occurred - homing failed
+    homingValid = false;
+    mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal);
+    state = HomingFailed;
+}
+
 } // namespace motion
 } // namespace modules

src/modules/movable_base.h

@@ -14,7 +14,9 @@ public:
     enum {
         Ready = 0, // intentionally set as zero in order to allow zeroing the Idler structure upon startup -> avoid explicit initialization code
         Moving,
-        Homing,
+        HomeForward,
+        HomeMoveAwayFromForward,
+        HomeBack,
         TMCFailed,
         HomingFailed
     };
@@ -31,7 +33,8 @@ public:
         , plannedSlot(-1)
         , currentSlot(-1)
         , homingValid(false)
-        , axis(axis) {}
+        , axis(axis)
+        , axisStart(0) {}
 
     /// virtual ~MovableBase(); intentionally disabled, see description in logic::CommandBase
 
@@ -79,9 +82,13 @@ protected:
 
     config::Axis axis;
 
+    int32_t axisStart;
+
     virtual void PrepareMoveToPlannedSlot() = 0;
-    virtual void PlanHomingMove() = 0;
-    virtual void FinishHomingAndPlanMoveToParkPos() = 0;
+    virtual void PlanHomingMoveForward() = 0;
+    virtual void PlanHomingMoveBack() = 0;
+    /// @returns true if the measured axis length is within the expected range, false otherwise
+    virtual bool FinishHomingAndPlanMoveToParkPos() = 0;
     virtual void FinishMove() = 0;
 
     OperationResult InitMovement();
@@ -91,7 +98,11 @@ protected:
 
     void PerformMove();
 
-    void PerformHome();
+    void PerformHomeForward();
+    void PerformMoveAwayFromForward();
+    void PerformHomeBack();
+
+    void HomeFailed();
 };
 
 } // namespace motion

src/modules/pulley.cpp

@@ -12,8 +12,9 @@ namespace pulley {
 
 Pulley pulley;
 
-void Pulley::FinishHomingAndPlanMoveToParkPos() {
+bool Pulley::FinishHomingAndPlanMoveToParkPos() {
     mm::motion.SetPosition(mm::Pulley, 0);
+    return true;
 }
 
 bool Pulley::Step() {
@@ -21,7 +22,7 @@ bool Pulley::Step() {
     case Moving:
         PerformMove();
         return false;
-    case Homing:
+    case HomeBack:
         homingValid = true;
         FinishHomingAndPlanMoveToParkPos();
         return true;

src/modules/pulley.h

@@ -36,8 +36,9 @@ public:
 
 protected:
     virtual void PrepareMoveToPlannedSlot() override {}
-    virtual void PlanHomingMove() override {}
-    virtual void FinishHomingAndPlanMoveToParkPos() override;
+    virtual void PlanHomingMoveForward() override {}
+    virtual void PlanHomingMoveBack() override {}
+    virtual bool FinishHomingAndPlanMoveToParkPos() override;
     virtual void FinishMove() override {}
 };
 

src/modules/selector.cpp

@@ -18,12 +18,26 @@ void Selector::PrepareMoveToPlannedSlot() {
     dbg_logic_fP(PSTR("Prepare Move Selector slot %d"), plannedSlot);
 }
 
-void Selector::PlanHomingMove() {
-    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
-    dbg_logic_P(PSTR("Plan Homing Selector"));
+void Selector::PlanHomingMoveForward() {
+    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+    dbg_logic_P(PSTR("Plan Homing Selector Forward"));
 }
 
-void Selector::FinishHomingAndPlanMoveToParkPos() {
+void Selector::PlanHomingMoveBack() {
+    // we expect that we are at the front end of the axis, set the expected axis' position
+    mm::motion.SetPosition(mm::Selector, 0);
+    axisStart = mm::stepsToUnit<mm::S_pos_t>(mm::S_pos_t({ mm::motion.CurPosition(mm::Selector) }));
+    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+    dbg_logic_P(PSTR("Plan Homing Selector Back"));
+}
+
+bool Selector::FinishHomingAndPlanMoveToParkPos() {
+    // check the axis' length
+    int32_t axisEnd = mm::stepsToUnit<mm::S_pos_t>(mm::S_pos_t({ mm::motion.CurPosition(mm::Selector) }));
+    if (abs(axisEnd - axisStart) < (config::selectorLimits.lenght.v - 3)) { //@@TODO is 3mm ok?
+        return false; // we couldn't home correctly, we cannot set the Selector's position
+    }
+
     mm::motion.SetPosition(mm::Selector, mm::unitToSteps<mm::S_pos_t>(config::selectorLimits.lenght));
     currentSlot = -1;
 
@@ -32,6 +46,7 @@ void Selector::FinishHomingAndPlanMoveToParkPos() {
         plannedSlot = IdleSlotIndex();
     }
     InitMovement();
+    return true;
 }
 
 void Selector::FinishMove() {
@@ -46,7 +61,7 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
     plannedSlot = slot;
 
     // if we are homing right now, just record the desired planned slot and return Accepted
-    if (state == Homing) {
+    if (state == HomeBack) {
         return OperationResult::Accepted;
     }
 
@@ -72,9 +87,16 @@ bool Selector::Step() {
         PerformMove();
         //dbg_logic_P(PSTR("Moving Selector"));
         return false;
-    case Homing:
+    case HomeForward:
+        dbg_logic_P(PSTR("Homing Selector Forward"));
+        PerformHomeForward();
+        return false;
+    case HomeMoveAwayFromForward:
+        PerformMoveAwayFromForward();
+        return false;
+    case HomeBack:
         dbg_logic_P(PSTR("Homing Selector"));
-        PerformHome();
+        PerformHomeBack();
         return false;
     case Ready:
         if (!homingValid && mg::globals.FilamentLoaded() < mg::InSelector) {

src/modules/selector.h

@@ -42,8 +42,9 @@ public:
 
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
-    virtual void PlanHomingMove() override;
-    virtual void FinishHomingAndPlanMoveToParkPos() override;
+    virtual void PlanHomingMoveForward() override;
+    virtual void PlanHomingMoveBack() override;
+    virtual bool FinishHomingAndPlanMoveToParkPos() override;
     virtual void FinishMove() override;
 
 private:

tests/unit/logic/stubs/main_loop_stub.cpp

@@ -88,13 +88,33 @@ void SimulateIdlerAndSelectorHoming() {
 
     mm::TriggerStallGuard(mm::Selector);
     mm::TriggerStallGuard(mm::Idler);
+    main_loop();
+    mm::motion.StallGuardReset(mm::Selector);
+    mm::motion.StallGuardReset(mm::Idler);
+
+    // now do a correct amount of steps of each axis towards the other end
+    uint32_t idlerSteps = mm::unitToSteps<mm::I_pos_t>(config::idlerLimits.lenght);
+    uint32_t selectorSteps = mm::unitToSteps<mm::S_pos_t>(config::selectorLimits.lenght);
+    uint32_t maxSteps = std::max(idlerSteps, selectorSteps) + 1;
+
+    for (uint32_t i = 0; i < maxSteps; ++i) {
+        main_loop();
+
+        if (i == idlerSteps) {
+            mm::TriggerStallGuard(mm::Idler);
+        } else {
+            mm::motion.StallGuardReset(mm::Idler);
+        }
+        if (i == selectorSteps) {
+            mm::TriggerStallGuard(mm::Selector);
+        } else {
+            mm::motion.StallGuardReset(mm::Selector);
+        }
+    }
 
     // now the Selector and Idler shall perform a move into their parking positions
     while (ms::selector.State() != mm::MovableBase::Ready || mi::idler.State() != mm::MovableBase::Ready)
         main_loop();
-
-    mm::motion.StallGuardReset(mm::Selector);
-    mm::motion.StallGuardReset(mm::Idler);
 }
 
 void SimulateIdlerHoming() {
@@ -104,12 +124,25 @@ void SimulateIdlerHoming() {
     }
 
     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 idlerSteps = mm::unitToSteps<mm::I_pos_t>(config::idlerLimits.lenght) + 1;
+
+    for (uint32_t i = 0; i < idlerSteps; ++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();
-
-    mm::motion.StallGuardReset(mm::Idler);
 }
 
 void SimulateSelectorHoming() {
@@ -119,12 +152,25 @@ void SimulateSelectorHoming() {
     }
 
     mm::TriggerStallGuard(mm::Selector);
+    main_loop();
+    mm::motion.StallGuardReset(mm::Selector);
+
+    // now do a correct amount of steps of each axis towards the other end
+    uint32_t selectorSteps = mm::unitToSteps<mm::S_pos_t>(config::selectorLimits.lenght) + 1;
+
+    for (uint32_t i = 0; i < selectorSteps; ++i) {
+        main_loop();
+
+        if (i == selectorSteps) {
+            mm::TriggerStallGuard(mm::Selector);
+        } else {
+            mm::motion.StallGuardReset(mm::Selector);
+        }
+    }
 
     // now the Selector shall perform a move into their parking positions
     while (ms::selector.State() != mm::MovableBase::Ready)
         main_loop();
-
-    mm::motion.StallGuardReset(mm::Selector);
 }
 
 void EnsureActiveSlotIndex(uint8_t slot, mg::FilamentLoadState loadState) {