diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 23aece3..95c3734 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -13,6 +13,10 @@ void Idler::PrepareMoveToPlannedSlot() { mm::motion.PlanMoveTo(SlotPosition(plannedSlot), mm::unitToAxisUnit(config::idlerFeedrate)); } +void Idler::PlanHomingMove() { + mm::motion.PlanMove(mm::unitToAxisUnit(-360.0_deg), mm::unitToAxisUnit(config::idlerFeedrate)); +} + Idler::OperationResult Idler::Disengage() { if (state == Moving) return OperationResult::Refused; @@ -43,8 +47,7 @@ bool Idler::Home() { if (state == Moving) return false; plannedEngage = false; - mm::motion.InitAxis(mm::Idler); - mm::motion.Home(mm::Idler, false); + PlanHome(mm::Idler); return true; } @@ -53,6 +56,9 @@ bool Idler::Step() { case Moving: PerformMove(mm::Idler); return false; + case Homing: + PerformHome(mm::Idler); + return false; case Ready: currentlyEngaged = plannedEngage; currentSlot = plannedSlot; diff --git a/src/modules/idler.h b/src/modules/idler.h index 5b9ef56..130632c 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -48,10 +48,9 @@ public: /// @returns the index of idle position of the idler, usually 5 in case of 0-4 valid indices of filament slots inline static constexpr uint8_t IdleSlotIndex() { return config::toolCount; } - // hal::tmc2130::MotorParams TMCDriverParams()const; - protected: virtual void PrepareMoveToPlannedSlot() override; + virtual void PlanHomingMove() override; private: /// direction of travel - engage/disengage diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 9363993..91fab93 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -1,9 +1,21 @@ #include "movable_base.h" +#include "globals.h" #include "motion.h" namespace modules { namespace motion { +void MovableBase::PlanHome(config::Axis axis) { + // switch to normal mode on this axis + state = Homing; + mm::motion.InitAxis(axis); + mm::motion.SetMode(axis, mm::Normal); + 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); +} + MovableBase::OperationResult MovableBase::InitMovement(config::Axis axis) { if (motion.InitAxis(axis)) { PrepareMoveToPlannedSlot(); @@ -26,5 +38,17 @@ void MovableBase::PerformMove(config::Axis axis) { } } +void MovableBase::PerformHome(config::Axis axis) { + if (mm::motion.StallGuard(axis)) { + // we have reached the end of the axis - homed ok + mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal); + state = Ready; + } else if (mm::motion.QueueEmpty(axis)) { + // we ran out of planned moves but no StallGuard event has occurred - homing failed + mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal); + state = Failed; + } +} + } // namespace motion } // namespace modules diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 82ca466..950d070 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -13,6 +13,7 @@ public: enum { Ready = 0, // intentionally set as zero in order to allow zeroing the Idler structure upon startup -> avoid explicit initialization code Moving, + Homing, Failed }; @@ -40,6 +41,9 @@ public: inline hal::tmc2130::ErrorFlags TMCErrorFlags() const { return tmcErrorFlags; } + /// Prepare a homing move of the axis + void PlanHome(config::Axis axis); + protected: /// internal state of the automaton uint8_t state; @@ -54,10 +58,13 @@ protected: hal::tmc2130::ErrorFlags tmcErrorFlags; virtual void PrepareMoveToPlannedSlot() = 0; + virtual void PlanHomingMove() = 0; OperationResult InitMovement(config::Axis axis); void PerformMove(config::Axis axis); + + void PerformHome(config::Axis axis); }; } // namespace motion diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index edd035d..d4bbb3e 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -13,6 +13,10 @@ void Selector::PrepareMoveToPlannedSlot() { mm::motion.PlanMoveTo(SlotPosition(plannedSlot), mm::unitToAxisUnit(config::selectorFeedrate)); } +void Selector::PlanHomingMove() { + mm::motion.PlanMove(mm::unitToAxisUnit(-100.0_mm), mm::unitToAxisUnit(config::selectorFeedrate)); +} + Selector::OperationResult Selector::MoveToSlot(uint8_t slot) { if (state == Moving) return OperationResult::Refused; @@ -28,8 +32,7 @@ Selector::OperationResult Selector::MoveToSlot(uint8_t slot) { bool Selector::Home() { if (state == Moving) return false; - mm::motion.InitAxis(mm::Selector); - mm::motion.Home(mm::Selector, false); + PlanHome(mm::Selector); return true; } @@ -38,6 +41,9 @@ bool Selector::Step() { case Moving: PerformMove(mm::Selector); return false; + case Homing: + PerformHome(mm::Selector); + return false; case Ready: currentSlot = plannedSlot; mm::motion.Disable(mm::Selector); // turn off selector motor's power every time diff --git a/src/modules/selector.h b/src/modules/selector.h index 95b9146..876b798 100644 --- a/src/modules/selector.h +++ b/src/modules/selector.h @@ -39,6 +39,7 @@ public: protected: virtual void PrepareMoveToPlannedSlot() override; + virtual void PlanHomingMove() override; private: };