diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cf92b2..bbb38c9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -195,7 +195,10 @@ target_sources( src/modules/finda.cpp src/modules/leds.cpp src/modules/motion.cpp - src/logic/mm_control.cpp + src/logic/command_base.cpp + src/logic/no_command.cpp + src/logic/unload_filament.cpp + src/logic/unload_to_finda.cpp ) set_property( diff --git a/src/logic/command_base.cpp b/src/logic/command_base.cpp new file mode 100644 index 0000000..084573b --- /dev/null +++ b/src/logic/command_base.cpp @@ -0,0 +1,5 @@ +#include "command_base.h" + +namespace logic { + +} // namespace logic diff --git a/src/logic/command_base.h b/src/logic/command_base.h new file mode 100644 index 0000000..c052798 --- /dev/null +++ b/src/logic/command_base.h @@ -0,0 +1,49 @@ +#pragma once +#include +#include "error_codes.h" +#include "progress_codes.h" + +/// Base class defining common API for high-level operations/commands/state machines +/// +/// Which state machines are high-level? Those which are being initiated either by a command over the serial line or from a button +/// - they report their progress to the printer +/// - they can be composed of other sub automatons + +namespace logic { + +/// Tasks derived from this base class are the top-level operations inhibited by the printer. +/// These tasks report their progress and only one of these tasks is allowed to run at once. +class CommandBase { +public: + inline CommandBase() + : state(ProgressCode::OK) + , error(ErrorCode::OK) {} + + // Normally, a base class should (must) have a virtual destructor to enable correct deallocation of superstructures. + // However, in our case we don't want ANY destruction of these objects and moreover - adding a destructor like this + // makes the linker complain about missing operator delete(), which is really not something we want/need in our case. + // Without the destructor, the linker is "happy" ;) + // virtual ~CommandBase() = default; + + /// resets the automaton + virtual void Reset() = 0; + + /// steps the state machine + /// @returns true if the automaton finished its work + virtual bool Step() = 0; + + /// @returns progress of operation - each automaton consists of several internal states + /// which should be reported to the user via the printer's LCD + /// E.g. Tool change: first tries to unload filament, then selects another slot and then tries to load filament + virtual ProgressCode State() const { return state; } + + /// @returns status of the operation - e.g. RUNNING, OK, or an error code if the operation failed + /// Please see @ErrorCode for more details + virtual ErrorCode Error() const { return error; } + +protected: + ProgressCode state; + ErrorCode error; +}; + +} // namespace logic diff --git a/src/logic/error_codes.h b/src/logic/error_codes.h new file mode 100644 index 0000000..2a4ec34 --- /dev/null +++ b/src/logic/error_codes.h @@ -0,0 +1,15 @@ +#pragma once +#include + +/// A complete set of error codes which may be a result of a high-level command/operation +/// This header file shall be included in the printer's firmware as well as a reference, +/// therefore the error codes have been extracted to one place + +enum class ErrorCode : int_fast8_t { + RUNNING = 0, ///< the operation is still running + OK, ///< the operation finished OK + + /// Unload Filament related error codes + UNLOAD_FINDA_DIDNT_TRIGGER = -1, ///< FINDA didn't trigger while unloading filament - either there is something blocking the metal ball or a cable is broken/disconnected + UNLOAD_ERROR2 = -2, +}; diff --git a/src/logic/mm_control.cpp b/src/logic/mm_control.cpp deleted file mode 100644 index 88afffc..0000000 --- a/src/logic/mm_control.cpp +++ /dev/null @@ -1,194 +0,0 @@ -#include "mm_control.h" -#include "../modules/motion.h" -#include "../modules/leds.h" -#include "../modules/buttons.h" -#include "../modules/finda.h" -#include "../modules/permanent_storage.h" - -namespace logic { - -// "small" state machines will serve as building blocks for high-level commands/operations -// - engage/disengage idler -// - rotate pulley to some direction as long as the FINDA is on/off -// - rotate some axis to some fixed direction -// - load/unload to finda -// - -// motion planning -// - we need some kind of planner buffer, especially because of accelerations -// because we may need to match the ramps between moves seamlessly - just like on a printer - -/// A "small" automaton example - Try to unload filament to FINDA and if it fails try to recover several times. -/// \dot -/// digraph example { -/// node [shape=record, fontname=Helvetica, fontsize=10]; -/// b [ label="class B" URL="\ref B"]; -/// c [ label="class C" URL="\ref C"]; -/// b -> c [ arrowhead="open", style="dashed" ]; -///} -///\enddot -struct UnloadToFinda { - enum { - WaitingForFINDA, - OK, - Failed - }; - uint8_t state; - uint8_t maxTries; - inline UnloadToFinda(uint8_t maxTries) - : maxTries(maxTries) { Reset(); } - - /// Restart the automaton - inline void 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 - } else { - // FINDA is sensing the filament, plan moves to unload it - int unloadSteps = /*BowdenLength::get() +*/ 1100; // @@TODO - const int second_point = unloadSteps - 1300; - // mm::motion.PlanMove(mm::Pulley, -1400, 6000); // @@TODO constants - // mm::motion.PlanMove(mm::Pulley, -1800 + 1400, 2500); // @@TODO constants 1800-1400 = 400 - // mm::motion.PlanMove(mm::Pulley, -second_point + 1800, 550); // @@TODO constants - state = WaitingForFINDA; - } - } - - /// @returns true if the state machine finished its job, false otherwise - bool Step() { - namespace mm = modules::motion; - namespace mf = modules::finda; - switch (state) { - case WaitingForFINDA: - if (modules::finda::finda.Pressed()) { - // detected end of filament - state = OK; - } else if (/*tmc2130_read_gstat() &&*/ mm::motion.QueueEmpty()) { - // we reached the end of move queue, but the FINDA didn't switch off - // two possible causes - grinded filament of malfunctioning FINDA - if (--maxTries) { - Reset(); // try again - } else { - state = Failed; - } - } - return false; - case OK: - case Failed: - default: - return true; - } - } -}; - -/// A high-level command state machine -/// Handles the complex logic of unloading filament -class UnloadFilament : public TaskBase { - enum State { - EngagingIdler, - UnloadingToFinda, - DisengagingIdler, - AvoidingGrind, - Finishing, - OK, - ERR1DisengagingIdler, - ERR1WaitingForUser - }; - - UnloadToFinda unl; - - inline UnloadFilament() - : TaskBase() - , unl(3) { Reset(); } - - /// Restart the automaton - void Reset() override { - namespace mm = modules::motion; - // unloads filament from extruder - filament is above Bondtech gears - mm::motion.InitAxis(mm::Pulley); - state = EngagingIdler; - mm::motion.Idler(mm::Engage); - } - - /// @returns true if the state machine finished its job, false otherwise - bool Step() override { - namespace mm = modules::motion; - switch (state) { - case EngagingIdler: // state 1 engage idler - if (mm::motion.IdlerEngaged()) { // if idler is in parked position un-park it get in contact with filament - state = UnloadingToFinda; - unl.Reset(); - } - return false; - case UnloadingToFinda: // state 2 rotate pulley as long as the FINDA is on - if (unl.Step()) { - if (unl.state == UnloadToFinda::Failed) { - // couldn't unload to FINDA, report error and wait for user to resolve it - state = ERR1DisengagingIdler; - // modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::blink0); - } else { - state = DisengagingIdler; - } - // in all cases disengage the idler - mm::motion.Idler(mm::Disengage); - } - return false; - case DisengagingIdler: - if (mm::motion.IdlerDisengaged()) { - state = AvoidingGrind; - // mm::motion.PlanMove(mm::Pulley, -100, 10); // @@TODO constants - } - return false; - case AvoidingGrind: // state 3 move a little bit so it is not a grinded hole in filament - if (mm::motion.QueueEmpty()) { - state = Finishing; - mm::motion.Idler(mm::Disengage); - return true; - } - return false; - case Finishing: - if (mm::motion.QueueEmpty()) { - state = OK; - mm::motion.DisableAxis(mm::Pulley); - } - return false; - case ERR1DisengagingIdler: // couldn't unload to FINDA - if (mm::motion.IdlerDisengaged()) { - state = ERR1WaitingForUser; - } - return false; - case ERR1WaitingForUser: { - // waiting for user buttons and/or a command from the printer - bool help = modules::buttons::buttons.ButtonPressed(modules::buttons::Left) /*|| command_help()*/; - bool tryAgain = modules::buttons::buttons.ButtonPressed(modules::buttons::Middle) /*|| command_tryAgain()*/; - bool userResolved = modules::buttons::buttons.ButtonPressed(modules::buttons::Right) /*|| command_userResolved()*/; - if (help) { - // try to manually unload just a tiny bit - help the filament with the pulley - //@@TODO - } else if (tryAgain) { - // try again the whole sequence - Reset(); - } else if (userResolved) { - // problem resolved - the user pulled the fillament by hand - // modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::off); - // modules::leds::leds.SetMode(active_extruder, modules::leds::green, modules::leds::on); - // mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants - state = AvoidingGrind; - } - return false; - } - case OK: - // isFilamentLoaded = false; // filament unloaded - return true; // successfully finished - } - } - - /// @returns progress of operation - virtual uint8_t Progress() const override { - return state; // for simplicity return state, will be more elaborate later in order to report the exact state of the MMU into the printer - } -}; - -} // namespace logic diff --git a/src/logic/mm_control.h b/src/logic/mm_control.h deleted file mode 100644 index 4ba47ea..0000000 --- a/src/logic/mm_control.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once -#include - -/// @@TODO @3d-gussner -/// Extract the current state machines of high-level operations (load fillament, unload fillament etc.) here -/// Design some nice non-blocking API for these operations -/// -/// Which automatons are high-level? Those which are being initiated either by a command over the serial line or from a button -/// - they report their progress to the printer -/// - they can be composed of other sub automatons - -namespace logic { - -/// Tasks derived from this base class are the top-level operations inhibited by the printer. -/// These tasks report their progress and only one of these tasks is allowed to run at once. -class TaskBase { -public: - inline TaskBase() = default; - - virtual void Reset() = 0; - virtual bool Step() = 0; - /// probably individual states of the automaton - virtual uint8_t Progress() const = 0; - /// @@TODO cleanup status codes - /// @returns 0 if the operation is still running - /// 1 if the operation finished OK - /// >=2 if the operation failed - the value is the error code - virtual int8_t Status() const = 0; - -protected: - uint8_t state; -}; - -class Logic { - -public: - inline Logic() = default; - - void UnloadFilament(); -}; - -} // namespace logic diff --git a/src/logic/no_command.cpp b/src/logic/no_command.cpp new file mode 100644 index 0000000..05bb4b5 --- /dev/null +++ b/src/logic/no_command.cpp @@ -0,0 +1,7 @@ +#include "no_command.h" + +namespace logic { + +NoCommand noCommand; + +} // namespace logic diff --git a/src/logic/no_command.h b/src/logic/no_command.h new file mode 100644 index 0000000..89d3dc1 --- /dev/null +++ b/src/logic/no_command.h @@ -0,0 +1,23 @@ +#pragma once +#include +#include "command_base.h" +#include "unload_to_finda.h" + +namespace logic { + +/// A dummy No-command operation just to make the init of the firmware consistent (and cleaner code during processing) +class NoCommand : public CommandBase { +public: + inline NoCommand() + : CommandBase() {} + + /// Restart the automaton + void Reset() override {} + + /// @returns true if the state machine finished its job, false otherwise + bool Step() override { return true; } +}; + +extern NoCommand noCommand; + +} // namespace logic diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h new file mode 100644 index 0000000..dc430cb --- /dev/null +++ b/src/logic/progress_codes.h @@ -0,0 +1,19 @@ +#pragma once +#include + +/// A complete set of progress codes which may be reported while running a high-level command/operation +/// This header file shall be included in the printer's firmware as well as a reference, +/// therefore the progress codes have been extracted to one place + +enum class ProgressCode : uint_fast8_t { + OK = 0, ///< finished ok + + /// Unload Filament related progress codes + EngagingIdler, + UnloadingToFinda, + DisengagingIdler, + AvoidingGrind, + FinishingMoves, + ERR1DisengagingIdler, + ERR1WaitingForUser +}; diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp new file mode 100644 index 0000000..fbe328f --- /dev/null +++ b/src/logic/unload_filament.cpp @@ -0,0 +1,95 @@ +#include "unload_filament.h" +#include "../modules/buttons.h" +#include "../modules/finda.h" +#include "../modules/leds.h" +#include "../modules/motion.h" +#include "../modules/permanent_storage.h" + +namespace logic { + +UnloadFilament unloadFilament; + +void UnloadFilament::Reset() { + namespace mm = modules::motion; + // unloads filament from extruder - filament is above Bondtech gears + mm::motion.InitAxis(mm::Pulley); + state = ProgressCode::EngagingIdler; + error = ErrorCode::OK; + mm::motion.Idler(mm::Engage); +} + +bool UnloadFilament::Step() { + namespace mm = modules::motion; + switch (state) { + case ProgressCode::EngagingIdler: // state 1 engage idler + if (mm::motion.IdlerEngaged()) { // if idler is in parked position un-park it get in contact with filament + state = ProgressCode::UnloadingToFinda; + unl.Reset(); + } + return false; + case ProgressCode::UnloadingToFinda: // state 2 rotate pulley as long as the FINDA is on + if (unl.Step()) { + if (unl.state == UnloadToFinda::Failed) { + // couldn't unload to FINDA, report error and wait for user to resolve it + state = ProgressCode::ERR1DisengagingIdler; + // modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::blink0); + } else { + state = ProgressCode::DisengagingIdler; + } + // in all cases disengage the idler + mm::motion.Idler(mm::Disengage); + } + return false; + case ProgressCode::DisengagingIdler: + if (mm::motion.IdlerDisengaged()) { + state = ProgressCode::AvoidingGrind; + // mm::motion.PlanMove(mm::Pulley, -100, 10); // @@TODO constants + } + return false; + case ProgressCode::AvoidingGrind: // state 3 move a little bit so it is not a grinded hole in filament + if (mm::motion.QueueEmpty()) { + state = ProgressCode::FinishingMoves; + mm::motion.Idler(mm::Disengage); + return true; + } + return false; + case ProgressCode::FinishingMoves: + if (mm::motion.QueueEmpty()) { + state = ProgressCode::OK; + mm::motion.DisableAxis(mm::Pulley); + } + return false; + case ProgressCode::ERR1DisengagingIdler: // couldn't unload to FINDA + error = ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER; + if (mm::motion.IdlerDisengaged()) { + state = ProgressCode::ERR1WaitingForUser; + } + return false; + case ProgressCode::ERR1WaitingForUser: { + // waiting for user buttons and/or a command from the printer + bool help = modules::buttons::buttons.ButtonPressed(modules::buttons::Left) /*|| command_help()*/; + bool tryAgain = modules::buttons::buttons.ButtonPressed(modules::buttons::Middle) /*|| command_tryAgain()*/; + bool userResolved = modules::buttons::buttons.ButtonPressed(modules::buttons::Right) /*|| command_userResolved()*/; + if (help) { + // try to manually unload just a tiny bit - help the filament with the pulley + //@@TODO + } else if (tryAgain) { + // try again the whole sequence + Reset(); + } else if (userResolved) { + // problem resolved - the user pulled the fillament by hand + // modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::off); + // modules::leds::leds.SetMode(active_extruder, modules::leds::green, modules::leds::on); + // mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants + state = ProgressCode::AvoidingGrind; + } + return false; + } + case ProgressCode::OK: + // isFilamentLoaded = false; // filament unloaded + return true; // successfully finished + } + return false; +} + +} // namespace logic diff --git a/src/logic/unload_filament.h b/src/logic/unload_filament.h new file mode 100644 index 0000000..f18081b --- /dev/null +++ b/src/logic/unload_filament.h @@ -0,0 +1,28 @@ +#pragma once +#include +#include "command_base.h" +#include "unload_to_finda.h" + +namespace logic { + +/// A high-level command state machine +/// Handles the complex logic of unloading filament +class UnloadFilament : public CommandBase { +public: + inline UnloadFilament() + : CommandBase() + , unl(3) { Reset(); } + + /// Restart the automaton + void Reset() override; + + /// @returns true if the state machine finished its job, false otherwise + bool Step() override; + +private: + UnloadToFinda unl; +}; + +extern UnloadFilament unloadFilament; + +} // namespace logic diff --git a/src/logic/unload_to_finda.cpp b/src/logic/unload_to_finda.cpp new file mode 100644 index 0000000..725ade9 --- /dev/null +++ b/src/logic/unload_to_finda.cpp @@ -0,0 +1,52 @@ +#include "unload_to_finda.h" +#include "../modules/motion.h" +#include "../modules/leds.h" +#include "../modules/buttons.h" +#include "../modules/finda.h" +#include "../modules/permanent_storage.h" + +namespace logic { + +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 + } else { + // FINDA is sensing the filament, plan moves to unload it + int unloadSteps = /*BowdenLength::get() +*/ 1100; // @@TODO + const int second_point = unloadSteps - 1300; + // mm::motion.PlanMove(mm::Pulley, -1400, 6000); // @@TODO constants + // mm::motion.PlanMove(mm::Pulley, -1800 + 1400, 2500); // @@TODO constants 1800-1400 = 400 + // mm::motion.PlanMove(mm::Pulley, -second_point + 1800, 550); // @@TODO constants + state = WaitingForFINDA; + } +} + +bool UnloadToFinda::Step() { + namespace mm = modules::motion; + namespace mf = modules::finda; + switch (state) { + case WaitingForFINDA: + if (modules::finda::finda.Pressed()) { + // detected end of filament + state = OK; + } else if (/*tmc2130_read_gstat() &&*/ mm::motion.QueueEmpty()) { + // we reached the end of move queue, but the FINDA didn't switch off + // two possible causes - grinded filament of malfunctioning FINDA + if (--maxTries) { + Reset(); // try again + } else { + state = Failed; + } + } + return false; + case OK: + case Failed: + default: + return true; + } +} + +} // namespace logic diff --git a/src/logic/unload_to_finda.h b/src/logic/unload_to_finda.h new file mode 100644 index 0000000..289982f --- /dev/null +++ b/src/logic/unload_to_finda.h @@ -0,0 +1,40 @@ +#pragma once +#include + +/// Unload to FINDA "small" state machine +/// "small" state machines will serve as building blocks for high-level commands/operations +/// - engage/disengage idler +/// - rotate pulley to some direction as long as the FINDA is on/off +/// - rotate some axis to some fixed direction +/// - load/unload to finda + +namespace logic { + +/// A "small" automaton example - Try to unload filament to FINDA and if it fails try to recover several times. +/// \dot +/// digraph example { +/// node [shape=record, fontname=Helvetica, fontsize=10]; +/// b [ label="class B" URL="\ref B"]; +/// c [ label="class C" URL="\ref C"]; +/// b -> c [ arrowhead="open", style="dashed" ]; +///} +///\enddot +struct UnloadToFinda { + enum { + WaitingForFINDA, + OK, + Failed + }; + uint8_t state; + uint8_t maxTries; + inline UnloadToFinda(uint8_t maxTries) + : maxTries(maxTries) { Reset(); } + + /// Restart the automaton + void Reset(); + + /// @returns true if the state machine finished its job, false otherwise + bool Step(); +}; + +} // namespace logic diff --git a/src/main.cpp b/src/main.cpp index 2bd4fdc..e259942 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,14 +13,14 @@ #include "modules/leds.h" #include "modules/protocol.h" -#include "logic/mm_control.h" +#include "logic/command_base.h" +#include "logic/no_command.h" +#include "logic/unload_filament.h" static modules::protocol::Protocol protocol; -//static modules::buttons::Buttons buttons; -//static modules::leds::LEDs leds; -// @@TODO we need a dummy noCommand to init the pointer with ... makes the rest of the code much better and safer -logic::TaskBase *currentCommand = nullptr; +logic::CommandBase *currentCommand = &logic::noCommand; + /// remember the request message that started the currently running command modules::protocol::RequestMsg currentCommandRq(modules::protocol::RequestMsgCodes::unknown, 0); @@ -121,7 +121,7 @@ void SendMessage(const modules::protocol::ResponseMsg &msg) { void PlanCommand(const modules::protocol::RequestMsg &rq) { namespace mp = modules::protocol; - if ((currentCommand == nullptr) || (currentCommand->Status() == 1)) { + if (currentCommand->Error() == ErrorCode::OK) { // we are allowed to start a new command switch (rq.code) { case mp::RequestMsgCodes::Cut: @@ -137,7 +137,7 @@ void PlanCommand(const modules::protocol::RequestMsg &rq) { // currentCommand = &toolCommand; break; case mp::RequestMsgCodes::Unload: - // currentCommand = &unloadCommand; + currentCommand = &logic::unloadFilament; break; default: // currentCommand = &noCommand; @@ -149,26 +149,22 @@ void PlanCommand(const modules::protocol::RequestMsg &rq) { void ReportRunningCommand() { namespace mp = modules::protocol; - if (!currentCommand) { - // @@TODO what to report after startup? - } else { - mp::ResponseMsgParamCodes commandStatus; - uint8_t value = 0; - switch (currentCommand->Status()) { - case 0: - commandStatus = mp::ResponseMsgParamCodes::Processing; - value = currentCommand->Progress(); - break; - case 1: - commandStatus = mp::ResponseMsgParamCodes::Finished; - break; - default: - commandStatus = mp::ResponseMsgParamCodes::Error; - value = currentCommand->Status() - 2; // @@TODO cleanup - break; - } - SendMessage(mp::ResponseMsg(currentCommandRq, commandStatus, value)); + mp::ResponseMsgParamCodes commandStatus; + uint8_t value = 0; + switch (currentCommand->Error()) { + case ErrorCode::RUNNING: + commandStatus = mp::ResponseMsgParamCodes::Processing; + value = (uint8_t)currentCommand->State(); + break; + case ErrorCode::OK: + commandStatus = mp::ResponseMsgParamCodes::Finished; + break; + default: + commandStatus = mp::ResponseMsgParamCodes::Error; + value = (uint8_t)currentCommand->Error(); + break; } + SendMessage(mp::ResponseMsg(currentCommandRq, commandStatus, value)); } void ProcessRequestMsg(const modules::protocol::RequestMsg &rq) { diff --git a/src/modules/motion.h b/src/modules/motion.h index 63198dd..de63c7e 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -83,7 +83,7 @@ public: void Step(); /// @returns true if all planned moves have been finished - bool QueueEmpty() const; + bool QueueEmpty() const { return false; } /// stop whatever moves are being done void AbortPlannedMoves() {}