diff --git a/CMakeLists.txt b/CMakeLists.txt index 79ed883..db9d487 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -193,6 +193,7 @@ target_sources( src/modules/buttons.cpp src/modules/debouncer.cpp src/modules/finda.cpp + src/modules/fsensor.cpp src/modules/idler.cpp src/modules/leds.cpp src/modules/motion.cpp @@ -200,6 +201,7 @@ target_sources( src/logic/command_base.cpp src/logic/cut_filament.cpp src/logic/eject_filament.cpp + src/logic/feed_to_bondtech.cpp src/logic/feed_to_finda.cpp src/logic/load_filament.cpp src/logic/no_command.cpp diff --git a/src/logic/cut_filament.cpp b/src/logic/cut_filament.cpp index 3490a62..ec3f471 100644 --- a/src/logic/cut_filament.cpp +++ b/src/logic/cut_filament.cpp @@ -46,7 +46,7 @@ bool CutFilament::Step() { 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: + case ErrorCode::FINDA_DIDNT_TRIGGER: break; } } @@ -54,10 +54,10 @@ bool CutFilament::Step() { case ProgressCode::SelectingFilamentSlot: if (mm::motion.QueueEmpty()) { // idler and selector finished their moves feed.Reset(true); - state = ProgressCode::FeedingToFINDA; + 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 + 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 @@ -97,7 +97,7 @@ bool CutFilament::Step() { break; case ProgressCode::ReturningSelector: if (mm::motion.QueueEmpty()) { // selector returned to position, feed the filament back to FINDA - state = ProgressCode::FeedingToFINDA; + state = ProgressCode::FeedingToFinda; feed.Reset(true); } break; diff --git a/src/logic/eject_filament.cpp b/src/logic/eject_filament.cpp index 09aa47d..6746a35 100644 --- a/src/logic/eject_filament.cpp +++ b/src/logic/eject_filament.cpp @@ -45,7 +45,7 @@ bool EjectFilament::Step() { 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: + case ErrorCode::FINDA_DIDNT_TRIGGER: break; } } diff --git a/src/logic/error_codes.h b/src/logic/error_codes.h index 2a4ec34..f1d6f3e 100644 --- a/src/logic/error_codes.h +++ b/src/logic/error_codes.h @@ -10,6 +10,7 @@ enum class ErrorCode : int_fast8_t { 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 + 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/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp new file mode 100644 index 0000000..512afc9 --- /dev/null +++ b/src/logic/feed_to_bondtech.cpp @@ -0,0 +1,59 @@ +#include "feed_to_bondtech.h" +#include "../modules/buttons.h" +#include "../modules/fsensor.h" +#include "../modules/idler.h" +#include "../modules/leds.h" +#include "../modules/motion.h" +#include "../modules/permanent_storage.h" + +namespace logic { + +namespace mm = modules::motion; +namespace mfs = modules::fsensor; +namespace mi = modules::idler; +namespace ml = modules::leds; +namespace mp = modules::permanent_storage; + +void FeedToBondtech::Reset(uint8_t maxRetries) { + state = EngagingIdler; + this->maxRetries = maxRetries; + mi::idler.Engage(0 /*active_extruder*/); // @@TODO +} + +bool FeedToBondtech::Step() { + const uint16_t steps = mp::BowdenLength::get(); + + switch (state) { + case EngagingIdler: + if (mi::idler.Engaged()) { + state = PushingFilament; + ml::leds.SetMode(0, ml::Color::green, ml::blink0); //@@TODO active slot index + mm::motion.PlanMove(steps, 0, 0, 4500, 0, 0); //@@TODO constants - there was some strange acceleration sequence in the original FW, + // we can probably hand over some array of constants for hand-tuned acceleration + leverage some smoothing in the stepper as well + } + return false; + case PushingFilament: + if (mfs::fsensor.Pressed()) { + mm::motion.AbortPlannedMoves(); // stop pushing filament + state = DisengagingIdler; + } else if (mm::motion.StallGuard(mm::Pulley)) { + // stall guard occurred during movement - the filament got stuck + state = Failed; // @@TODO may be even report why it failed + } else if (mm::motion.QueueEmpty()) { // all moves have been finished and the fsensor didn't switch on + state = Failed; + } + return false; + case DisengagingIdler: + if (!mi::idler.Engaged()) { + state = OK; + ml::leds.SetMode(0, ml::Color::green, ml::on); //@@TODO active slot index + } + return false; + case OK: + case Failed: + default: + return true; + } +} + +} // namespace logic diff --git a/src/logic/feed_to_bondtech.h b/src/logic/feed_to_bondtech.h new file mode 100644 index 0000000..887815d --- /dev/null +++ b/src/logic/feed_to_bondtech.h @@ -0,0 +1,40 @@ +#pragma once +#include + +/// @brief Feed filament to Bondtech gears of the printer +/// +/// Continuously feed filament until the printer detects the filament in its filament sensor + +namespace logic { + +struct FeedToBondtech { + enum { + EngagingIdler, + PushingFilament, + UnloadBackToPTFE, + DisengagingIdler, + OK, + Failed + }; + + inline FeedToBondtech() + : state(OK) + , maxRetries(1) {} + + /// Restart the automaton + void Reset(uint8_t maxRetries); + + /// @returns true if the state machine finished its job, false otherwise + bool Step(); + + /// This method may be used to check the result of the automaton + /// @returns OK if everything went OK and printer's filament sensor triggered + /// @returns Failed if the maximum feed length has been reached without the the printer's fsensor trigger being reported + inline uint8_t State() const { return state; } + +private: + uint8_t state; + uint8_t maxRetries; +}; + +} // namespace logic diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index 917f056..97c978b 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -1,25 +1,213 @@ #include "load_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 { LoadFilament loadFilament; namespace mm = modules::motion; +namespace mi = modules::idler; +namespace ms = modules::selector; +namespace mf = modules::finda; +namespace ml = modules::leds; void LoadFilament::Reset(uint8_t param) { state = ProgressCode::EngagingIdler; error = ErrorCode::OK; + mi::idler.Engage(param); // @@TODO } bool LoadFilament::Step() { switch (state) { + case ProgressCode::EngagingIdler: + if (mi::idler.Engaged()) { + mm::motion.InitAxis(mm::Pulley); + state = ProgressCode::FeedingToFinda; + feed.Reset(true); + } + break; + case ProgressCode::FeedingToFinda: + if (feed.Step()) { + if (feed.State() == FeedToFinda::Failed) { + // @@TODO - try to repeat 6x - push/pull sequence - probably something to put into feed_to_finda as an option + state = ProgressCode::ERR1DisengagingIdler; + mi::idler.Disengage(); + ml::leds.SetMode(0, ml::Color::red, ml::Mode::blink0); // signal loading error //@@TODO slot index + } else { + state = ProgressCode::FeedingToBondtech; + james.Reset(2); + } + } + break; + case ProgressCode::FeedingToBondtech: + if (james.Step()) { + switch (james.State()) { + case FeedToBondtech::Failed: + + case FeedToBondtech::OK: + mm::motion.DisableAxis(mm::Pulley); + mi::idler.Disengage(); + state = ProgressCode::DisengagingIdler; + } + } + case ProgressCode::DisengagingIdler: + if (!mi::idler.Engaged()) { + state = ProgressCode::OK; + } + break; + case ProgressCode::OK: + return true; + case ProgressCode::ERR1DisengagingIdler: // couldn't unload to FINDA + error = ErrorCode::FINDA_DIDNT_TRIGGER; + if (!mi::idler.Engaged()) { + 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 load just a tiny bit - help the filament with the pulley + //@@TODO + } else if (tryAgain) { + // try again the whole sequence + Reset(0); // @@TODO param + } 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; + } } return false; } +//! @brief Load filament through bowden +//! @param disengageIdler +//! * true Disengage idler after movement +//! * false Do not disengage idler after movement +//void load_filament_withSensor(bool disengageIdler) +//{ +// FilamentLoaded::set(active_extruder); +// motion_engage_idler(); + +// tmc2130_init_axis(AX_PUL, tmc2130_mode); + +// set_pulley_dir_push(); + +// int _loadSteps = 0; +// int _endstop_hit = 0; + +// // load filament until FINDA senses end of the filament, means correctly loaded into the selector +// // we can expect something like 570 steps to get in sensor +// do{ +// do_pulley_step(); +// _loadSteps++; +// delayMicroseconds(5500); +// } while (digitalRead(A1) == 0 && _loadSteps < 1500); + +// // filament did not arrive at FINDA, let's try to correct that +// if (digitalRead(A1) == 0){ +// for (int i = 6; i > 0; i--){ +// if (digitalRead(A1) == 0){ +// // attempt to correct +// set_pulley_dir_pull(); +// for (int i = 200; i >= 0; i--){ +// do_pulley_step(); +// delayMicroseconds(1500); +// } + +// set_pulley_dir_push(); +// _loadSteps = 0; +// do{ +// do_pulley_step(); +// _loadSteps++; +// delayMicroseconds(4000); +// if (digitalRead(A1) == 1) _endstop_hit++; +// } while (_endstop_hit<100 && _loadSteps < 500); +// } +// } +// } + +// // still not at FINDA, error on loading, let's wait for user input +// if (digitalRead(A1) == 0){ +// bool _continue = false; +// bool _isOk = false; +// motion_disengage_idler(); +// do{ +// if (!_isOk){ +// signal_load_failure(); +// }else{ +// signal_ok_after_load_failure(); +// } + +// switch (buttonPressed()){ +// case Btn::left: +// // just move filament little bit +// motion_engage_idler(); +// set_pulley_dir_push(); + +// for (int i = 0; i < 200; i++) +// { +// do_pulley_step(); +// delayMicroseconds(5500); +// } +// motion_disengage_idler(); +// break; +// case Btn::middle: +// // check if everything is ok +// motion_engage_idler(); +// _isOk = checkOk(); +// motion_disengage_idler(); +// break; +// case Btn::right: +// // continue with loading +// motion_engage_idler(); +// _isOk = checkOk(); +// motion_disengage_idler(); + +// if (_isOk) //pridat do podminky flag ze od tiskarny prislo continue +// { +// _continue = true; +// } +// break; +// default: +// break; +// } + +// } while ( !_continue ); + +// motion_engage_idler(); +// set_pulley_dir_push(); +// _loadSteps = 0; +// do +// { +// do_pulley_step(); +// _loadSteps++; +// delayMicroseconds(5500); +// } while (digitalRead(A1) == 0 && _loadSteps < 1500); +// // ? +// } +// else +// { +// // nothing +// } + +// motion_feed_to_bondtech(); + +// tmc2130_disable_axis(AX_PUL, tmc2130_mode); +// if (disengageIdler) motion_disengage_idler(); +// isFilamentLoaded = true; // filament loaded + } // namespace logic diff --git a/src/logic/load_filament.h b/src/logic/load_filament.h index 53c4ecc..d123b6d 100644 --- a/src/logic/load_filament.h +++ b/src/logic/load_filament.h @@ -1,7 +1,8 @@ #pragma once #include #include "command_base.h" -#include "unload_to_finda.h" +#include "feed_to_finda.h" +#include "feed_to_bondtech.h" namespace logic { @@ -19,6 +20,8 @@ public: bool Step() override; private: + FeedToFinda feed; + FeedToBondtech james; // bond ;) }; extern LoadFilament loadFilament; diff --git a/src/logic/progress_codes.h b/src/logic/progress_codes.h index 2b4cd03..1a4a6a4 100644 --- a/src/logic/progress_codes.h +++ b/src/logic/progress_codes.h @@ -8,19 +8,20 @@ enum class ProgressCode : uint_fast8_t { OK = 0, ///< finished ok - /// Unload Filament related progress codes EngagingIdler, - UnloadingToFinda, DisengagingIdler, + UnloadingToFinda, + FeedingToFinda, + FeedingToBondtech, AvoidingGrind, FinishingMoves, + ERR1DisengagingIdler, ERR1WaitingForUser, UnloadingFilament, LoadingFilament, SelectingFilamentSlot, - FeedingToFINDA, PreparingBlade, PushingFilament, PerformingCut, diff --git a/src/logic/tool_change.cpp b/src/logic/tool_change.cpp index ee46ea6..d97245b 100644 --- a/src/logic/tool_change.cpp +++ b/src/logic/tool_change.cpp @@ -43,7 +43,7 @@ bool ToolChange::Step() { load.Reset(plannedSlot); break; case ErrorCode::UNLOAD_ERROR2: // @@TODO what shall we do in case of this error? - case ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER: + case ErrorCode::FINDA_DIDNT_TRIGGER: break; } } diff --git a/src/logic/unload_filament.cpp b/src/logic/unload_filament.cpp index ac8a9fc..6f2b56e 100644 --- a/src/logic/unload_filament.cpp +++ b/src/logic/unload_filament.cpp @@ -62,7 +62,7 @@ bool UnloadFilament::Step() { } return false; case ProgressCode::ERR1DisengagingIdler: // couldn't unload to FINDA - error = ErrorCode::UNLOAD_FINDA_DIDNT_TRIGGER; + error = ErrorCode::FINDA_DIDNT_TRIGGER; if (!mi::idler.Engaged()) { state = ProgressCode::ERR1WaitingForUser; } diff --git a/src/main.cpp b/src/main.cpp index 5183950..7c0ad78 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -10,6 +10,7 @@ #include "modules/buttons.h" #include "modules/finda.h" +#include "modules/fsensor.h" #include "modules/idler.h" #include "modules/leds.h" #include "modules/protocol.h" @@ -205,6 +206,9 @@ void ProcessRequestMsg(const modules::protocol::RequestMsg &rq) { case mp::RequestMsgCodes::Unload: PlanCommand(rq); break; + // case mp::RequestMsgCodes::SetVar: //@@TODO - this is where e.g. printer's fsensor gets updated + // SetVar(rq); + // break; default: // respond with an error message break; @@ -252,6 +256,7 @@ void loop() { modules::buttons::buttons.Step(hal::adc::ReadADC(0)); modules::leds::leds.Step(0); modules::finda::finda.Step(0); + modules::fsensor::fsensor.Step(0); modules::idler::idler.Step(); modules::selector::selector.Step(); currentCommand->Step(); diff --git a/src/modules/fsensor.cpp b/src/modules/fsensor.cpp new file mode 100644 index 0000000..518becd --- /dev/null +++ b/src/modules/fsensor.cpp @@ -0,0 +1,17 @@ +#include "fsensor.h" + +namespace modules { +namespace fsensor { + +FSensor fsensor; + +void FSensor::Step(uint16_t time) { + debounce::Debouncer::Step(time, reportedFSensorState); +} + +void FSensor::ProcessMessage(bool on) { + reportedFSensorState = on; +} + +} // namespace finda +} // namespace modules diff --git a/src/modules/fsensor.h b/src/modules/fsensor.h new file mode 100644 index 0000000..f62fa9e --- /dev/null +++ b/src/modules/fsensor.h @@ -0,0 +1,30 @@ +#pragma once +#include +#include "debouncer.h" + +/// External module - model of printer's fsensor + +namespace modules { +namespace fsensor { + +/// the debouncer is probably not necessary, but it has all the necessary functionality for modelling of the fsensor + +class FSensor : protected debounce::Debouncer { +public: + inline FSensor() + : debounce::Debouncer(debounce) {}; + void Step(uint16_t time); + using debounce::Debouncer::Pressed; + + void ProcessMessage(bool on); + +private: + /// time interval for debouncing @@TODO specify units + constexpr static const uint16_t debounce = 10; + bool reportedFSensorState; ///< reported state that came from the printer via a communication message +}; + +extern FSensor fsensor; + +} // namespace finda +} // namespace modules diff --git a/src/modules/idler.h b/src/modules/idler.h index 059bb7c..a2b77f4 100644 --- a/src/modules/idler.h +++ b/src/modules/idler.h @@ -17,9 +17,9 @@ public: }; inline Idler() - : plannedEngage(false) + : state(Ready) + , plannedEngage(false) , plannedSlot(0) - , state(Ready) , currentSlot(0) , currentlyEngaged(false) {} diff --git a/src/modules/motion.h b/src/modules/motion.h index 748ce38..cdd4a35 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -68,6 +68,12 @@ public: /// Disable axis motor void DisableAxis(Axis axis) {} + /// @returns true if a stall guard event occurred recently on the axis + bool StallGuard(Axis axis) { return false; } + + /// clear stall guard flag reported on an axis + void ClearStallGuardFlag(Axis axis) {} + /// Enqueue move of a specific motor/axis into planner buffer /// @param pulley, idler, selector - target coords void PlanMove(uint16_t pulley, uint16_t idler, uint16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed); diff --git a/src/modules/selector.h b/src/modules/selector.h index 68fc630..7ec09a6 100644 --- a/src/modules/selector.h +++ b/src/modules/selector.h @@ -18,8 +18,8 @@ public: inline Selector() : state(Ready) - , currentSlot(0) - , plannedSlot(0) {} + , plannedSlot(0) + , currentSlot(0) {} // public operations on the selector