diff --git a/src/hal/tmc2130.h b/src/hal/tmc2130.h index f78ede8..68af305 100644 --- a/src/hal/tmc2130.h +++ b/src/hal/tmc2130.h @@ -45,6 +45,7 @@ struct __attribute__((packed)) ErrorFlags { , s2g(0) , otpw(0) , ot(0) {} + inline bool Good() const { return reset_flag == 0 && uv_cp == 0 && s2g == 0 && otpw == 0 && ot == 0; } }; /// TMC2130 interface - instances of this class are hidden in modules::motion::Motion::AxisData diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index c27c0a7..23aece3 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -51,10 +51,7 @@ bool Idler::Home() { bool Idler::Step() { switch (state) { case Moving: - if (mm::motion.QueueEmpty(mm::Idler)) { - // move finished - state = Ready; - } + PerformMove(mm::Idler); return false; case Ready: currentlyEngaged = plannedEngage; diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 8450827..52d8378 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -15,5 +15,15 @@ MovableBase::OperationResult MovableBase::InitMovement(config::Axis axis) { } } +void MovableBase::PerformMove(config::Axis axis) { + if (!mm::motion.DriverForAxis(axis).GetErrorFlags().Good()) { + // TMC2130 entered some error state, the planned move couldn't have been finished - result of operation is Failed + state = Failed; + } else if (mm::motion.QueueEmpty(axis)) { + // move finished + state = Ready; + } +} + } // namespace motion } // namespace modules diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 12033ea..d02d9ca 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -50,6 +50,8 @@ protected: virtual void PrepareMoveToPlannedSlot() = 0; OperationResult InitMovement(config::Axis axis); + + void PerformMove(config::Axis axis); }; } // namespace motion diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index 9cb77b0..edd035d 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -36,10 +36,7 @@ bool Selector::Home() { bool Selector::Step() { switch (state) { case Moving: - if (mm::motion.QueueEmpty(mm::Selector)) { - // move finished - state = Ready; - } + PerformMove(mm::Selector); return false; case Ready: currentSlot = plannedSlot;