diff --git a/CMakeLists.txt b/CMakeLists.txt index fce1efb..79ed883 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,11 +193,14 @@ target_sources( src/modules/buttons.cpp src/modules/debouncer.cpp src/modules/finda.cpp + src/modules/idler.cpp src/modules/leds.cpp src/modules/motion.cpp + src/modules/selector.cpp src/logic/command_base.cpp src/logic/cut_filament.cpp src/logic/eject_filament.cpp + src/logic/feed_to_finda.cpp src/logic/load_filament.cpp src/logic/no_command.cpp src/logic/tool_change.cpp diff --git a/src/logic/cut_filament.cpp b/src/logic/cut_filament.cpp index 92f7eeb..eff367a 100644 --- a/src/logic/cut_filament.cpp +++ b/src/logic/cut_filament.cpp @@ -1,23 +1,106 @@ #include "cut_filament.h" #include "../modules/buttons.h" #include "../modules/finda.h" +#include "../modules/idler.h" #include "../modules/leds.h" #include "../modules/motion.h" #include "../modules/permanent_storage.h" +#include "../modules/selector.h" namespace logic { CutFilament cutFilament; +namespace mm = modules::motion; +namespace mi = modules::idler; +namespace ms = modules::selector; + void CutFilament::Reset() { - namespace mm = modules::motion; - state = ProgressCode::EngagingIdler; error = ErrorCode::OK; + + bool isFilamentLoaded = true; //@@TODO + + if (isFilamentLoaded) { + state = ProgressCode::CutUnloadingFilament; + unl.Reset(); + } else { + SelectFilamentSlot(); + } +} + +void CutFilament::SelectFilamentSlot() { + state = ProgressCode::SelectingFilamentSlot; + uint8_t newFilamentSlot = 0; //@@TODO + mi::idler.Engage(newFilamentSlot); + ms::selector.MoveToSlot(newFilamentSlot); } bool CutFilament::Step() { - namespace mm = modules::motion; + const int cut_steps_pre = 700; + const int cut_steps_post = 150; + switch (state) { + case ProgressCode::CutUnloadingFilament: + if (unl.Step()) { + // unloading sequence finished + switch (unl.Error()) { + case ErrorCode::OK: // finished successfully + case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error? + case ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER: + break; + } + } + break; + case ProgressCode::SelectingFilamentSlot: + if (mm::motion.QueueEmpty()) { // idler and selector finished their moves + feed.Reset(true); + state = ProgressCode::FeedingToFINDA; + } + break; + case ProgressCode::FeedingToFINDA: // @@TODO this state will be reused for repeated cutting of filament ... probably there will be multiple attempts, not sure + if (feed.Step()) { + if (feed.State() == FeedToFinda::Failed) { + // @@TODO + } else { + // move selector aside - prepare the blade into active position + state = ProgressCode::PreparingBlade; + uint8_t newFilamentSlot = 1; //@@TODO + ms::selector.MoveToSlot(newFilamentSlot + 1); + } + } + break; + case ProgressCode::PreparingBlade: + if (mm::motion.QueueEmpty()) { + state = ProgressCode::EngagingIdler; + uint8_t newFilamentSlot = 0; //@@TODO + mi::idler.Engage(newFilamentSlot); + } + break; + case ProgressCode::EngagingIdler: + if (mi::idler.Engaged()) { + state = ProgressCode::PushingFilament; + mm::motion.PlanMove(cut_steps_pre, 0, 0, 1500, 0, 0); //@@TODO + } + break; + case ProgressCode::PushingFilament: + if (mm::motion.QueueEmpty()) { + state = ProgressCode::PerformingCut; + ms::selector.MoveToSlot(0); + } + break; + case ProgressCode::PerformingCut: + if (mm::motion.QueueEmpty()) { // this may not be necessary if we want the selector and pulley move at once + state = ProgressCode::ReturningSelector; + uint8_t newFilamentSlot = 0; //@@TODO + ms::selector.MoveToSlot(newFilamentSlot); // return selector back + } + break; + case ProgressCode::ReturningSelector: + if (mm::motion.QueueEmpty()) { // selector returned to position, feed the filament back to FINDA + state = ProgressCode::FeedingToFINDA; + feed.Reset(true); + } + break; } return false; } diff --git a/src/logic/cut_filament.h b/src/logic/cut_filament.h index 1c6292f..836b322 100644 --- a/src/logic/cut_filament.h +++ b/src/logic/cut_filament.h @@ -1,7 +1,8 @@ #pragma once #include #include "command_base.h" -#include "unload_to_finda.h" +#include "unload_filament.h" +#include "feed_to_finda.h" namespace logic { @@ -19,6 +20,10 @@ public: bool Step() override; private: + UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well + FeedToFinda feed; + + void SelectFilamentSlot(); }; extern CutFilament cutFilament; diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h index dc430cb..a5bc43e 100644 --- a/src/logic/progress_codes.h +++ b/src/logic/progress_codes.h @@ -15,5 +15,13 @@ enum class ProgressCode : uint_fast8_t { AvoidingGrind, FinishingMoves, ERR1DisengagingIdler, - ERR1WaitingForUser + ERR1WaitingForUser, + + CutUnloadingFilament, + SelectingFilamentSlot, + FeedingToFINDA, + PreparingBlade, + PushingFilament, + PerformingCut, + ReturningSelector, }; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index 8522e75..556e0ee 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -10,8 +10,10 @@ namespace logic { UnloadFilament unloadFilament; +namespace mm = modules::motion; +namespace mi = modules::idler; + void UnloadFilament::Reset() { - namespace mm = modules::motion; // unloads filament from extruder - filament is above Bondtech gears mm::motion.InitAxis(mm::Pulley); state = ProgressCode::EngagingIdler; @@ -20,8 +22,6 @@ void UnloadFilament::Reset() { } bool UnloadFilament::Step() { - namespace mm = modules::motion; - namespace mi = modules::idler; switch (state) { case ProgressCode::EngagingIdler: // state 1 engage idler if (mi::idler.Engaged()) { // if idler is in parked position un-park it get in contact with filament diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h index f18081b..29b2c46 100644 --- a/src/logic/unload_filament.h +++ b/src/logic/unload_filament.h @@ -11,7 +11,7 @@ class UnloadFilament : public CommandBase { public: inline UnloadFilament() : CommandBase() - , unl(3) { Reset(); } + , unl(3) {} /// Restart the automaton void Reset() override; diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp index 725ade9..b120fd9 100644 --- a/src/logic/unload_to_finda.cpp +++ b/src/logic/unload_to_finda.cpp @@ -1,15 +1,16 @@ #include "unload_to_finda.h" -#include "../modules/motion.h" -#include "../modules/leds.h" #include "../modules/buttons.h" #include "../modules/finda.h" +#include "../modules/leds.h" +#include "../modules/motion.h" #include "../modules/permanent_storage.h" namespace logic { +namespace mm = modules::motion; +namespace mf = modules::finda; + void UnloadToFinda::Reset() { - namespace mm = modules::motion; - namespace mf = modules::finda; // check the inital state of FINDA and plan the moves if (mf::finda.Pressed()) { state = OK; // FINDA is already off, we assume the fillament is not there, i.e. already unloaded @@ -25,8 +26,6 @@ void UnloadToFinda::Reset() { } bool UnloadToFinda::Step() { - namespace mm = modules::motion; - namespace mf = modules::finda; switch (state) { case WaitingForFINDA: if (modules::finda::finda.Pressed()) { diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index bad83da..3dbc8c6 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -7,12 +7,19 @@ namespace modules { namespace idler { +Idler idler; + namespace mm = modules::motion; bool Idler::Disengage() { if (state == Moving) return false; + + if (!Engaged()) + return true; + plannedEngage = false; + mm::motion.InitAxis(mm::Idler); // plan move to idle position // mm::motion.PlanMove(0, idle_position, 0, 1000, 0, 0); // @@TODO state = Moving; @@ -22,8 +29,13 @@ bool Idler::Disengage() { bool Idler::Engage(uint8_t slot) { if (state == Moving) return false; + + if (Engaged()) + return true; + plannedSlot = slot; plannedEngage = true; + mm::motion.InitAxis(mm::Idler); // mm::motion.PlanMove(0, slotPositions[slot], 0, 1000, 0, 0); // @@TODO state = Moving; return true; @@ -33,6 +45,7 @@ bool Idler::Home() { if (state == Moving) return false; plannedEngage = false; + mm::motion.InitAxis(mm::Idler); mm::motion.Home(mm::Idler, false); return true; } @@ -48,6 +61,10 @@ bool Idler::Step() { case Ready: currentlyEngaged = plannedEngage; currentSlot = plannedSlot; + + if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary) + mm::motion.DisableAxis(mm::Idler); + return true; case Failed: default: diff --git a/src/modules/idler.h b/src/modules/idler.h index dc76d65..059bb7c 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -16,6 +16,13 @@ public: Failed }; + inline Idler() + : plannedEngage(false) + , plannedSlot(0) + , state(Ready) + , currentSlot(0) + , currentlyEngaged(false) {} + // public operations on the idler /// @retuns false in case an operation is already underway @@ -47,13 +54,6 @@ private: /// current state uint8_t currentSlot; bool currentlyEngaged; - - inline Idler() - : plannedEngage(false) - , plannedSlot(0) - , state(Ready) - , currentSlot(0) - , currentlyEngaged(false) {} }; extern Idler idler; diff --git a/src/modules/motion.h b/src/modules/motion.h index 5c9819c..748ce38 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -61,7 +61,8 @@ public: /// @@TODO this is subject of discussion and change in the future class Motion { public: - /// Init axis driver + /// Init axis driver - @@TODO this should be probably hidden somewhere deeper ... something should manage the axes and their state + /// especially when the TMC may get randomly reset (deinited) void InitAxis(Axis axis) {} /// Disable axis motor diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp index f9ce348..d518d4d 100644 --- a/src/modules/selector.cpp +++ b/src/modules/selector.cpp @@ -7,12 +7,19 @@ namespace modules { namespace selector { +Selector selector; + namespace mm = modules::motion; bool Selector::MoveToSlot(uint8_t slot) { if (state == Moving) return false; + + if (currentSlot == slot) + return true; + plannedSlot = slot; + mm::motion.InitAxis(mm::Selector); // mm::motion.PlanMove(1, slotPositions[slot], 0, 1000, 0, 0); // @@TODO state = Moving; return true; @@ -21,6 +28,7 @@ bool Selector::MoveToSlot(uint8_t slot) { bool Selector::Home() { if (state == Moving) return false; + mm::motion.InitAxis(mm::Selector); mm::motion.Home(mm::Selector, false); return true; } @@ -35,6 +43,7 @@ bool Selector::Step() { return false; case Ready: currentSlot = plannedSlot; + mm::motion.DisableAxis(mm::Selector); // turn off selector motor's power every time return true; case Failed: default: diff --git a/src/modules/selector.h b/src/modules/selector.h index 9edf74e..68fc630 100644 --- a/src/modules/selector.h +++ b/src/modules/selector.h @@ -16,6 +16,11 @@ public: Failed }; + inline Selector() + : state(Ready) + , currentSlot(0) + , plannedSlot(0) {} + // public operations on the selector /// @retuns false in case an operation is already underway @@ -40,11 +45,6 @@ private: /// current state uint8_t currentSlot; - - inline Selector() - : state(Ready) - , currentSlot(0) - , plannedSlot(0) {} }; extern Selector selector;