From f9addb0d7a1f1d927c0b0f0fc594183ddd6ada6c Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Sat, 22 Jan 2022 10:58:30 +0100 Subject: [PATCH] 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 | 40 +++++++++++---- src/modules/idler.h | 5 +- src/modules/movable_base.cpp | 53 +++++++++++++++----- src/modules/movable_base.h | 21 ++++++-- src/modules/pulley.cpp | 5 +- src/modules/pulley.h | 5 +- src/modules/selector.cpp | 36 +++++++++++--- src/modules/selector.h | 5 +- tests/unit/logic/stubs/main_loop_stub.cpp | 60 ++++++++++++++++++++--- 9 files changed, 183 insertions(+), 47 deletions(-) diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 9735435..5679001 100644 --- a/src/modules/idler.cpp +++ b/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::unitToAxisUnit(-config::idlerLimits.lenght * 2), mm::unitToAxisUnit(config::idlerFeedrate)); - dbg_logic_P(PSTR("Plan Homing Idler")); +void Idler::PlanHomingMoveForward() { + mm::motion.PlanMove(mm::unitToAxisUnit(config::idlerLimits.lenght * 2), mm::unitToAxisUnit(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(config::idlerLimits.lenght)); + axisStart = mm::stepsToUnit(mm::I_pos_t({ mm::motion.CurPosition(mm::Idler) })); + mm::motion.PlanMove(mm::unitToAxisUnit(-config::idlerLimits.lenght * 2), mm::unitToAxisUnit(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::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) { diff --git a/src/modules/idler.h b/src/modules/idler.h index 11a7529..17a89b4 100644 --- a/src/modules/idler.h +++ b/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: diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 11e2f49..d4818fe 100644 --- a/src/modules/movable_base.cpp +++ b/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 diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 14a8ead..999ff36 100644 --- a/src/modules/movable_base.h +++ b/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 diff --git a/src/modules/pulley.cpp b/src/modules/pulley.cpp index e3678d8..8e4e1c5 100644 --- a/src/modules/pulley.cpp +++ b/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; diff --git a/src/modules/pulley.h b/src/modules/pulley.h index 8f3f217..73a2f0b 100644 --- a/src/modules/pulley.h +++ b/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 {} }; diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index 09af3bd..6184f9d 100644 --- a/src/modules/selector.cpp +++ b/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::unitToAxisUnit(config::selectorLimits.lenght * 2), mm::unitToAxisUnit(config::selectorFeedrate)); - dbg_logic_P(PSTR("Plan Homing Selector")); +void Selector::PlanHomingMoveForward() { + mm::motion.PlanMove(mm::unitToAxisUnit(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit(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::motion.CurPosition(mm::Selector) })); + mm::motion.PlanMove(mm::unitToAxisUnit(config::selectorLimits.lenght * 2), mm::unitToAxisUnit(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::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(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) { diff --git a/src/modules/selector.h b/src/modules/selector.h index 3170323..fd6a272 100644 --- a/src/modules/selector.h +++ b/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: diff --git a/tests/unit/logic/stubs/main_loop_stub.cpp b/tests/unit/logic/stubs/main_loop_stub.cpp index b59b6aa..d331531 100644 --- a/tests/unit/logic/stubs/main_loop_stub.cpp +++ b/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(config::idlerLimits.lenght); + uint32_t selectorSteps = mm::unitToSteps(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(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(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) {