diff --git a/src/hal/avr/shr16.cpp b/src/hal/avr/shr16.cpp index 50cda5e..c024be1 100644 --- a/src/hal/avr/shr16.cpp +++ b/src/hal/avr/shr16.cpp @@ -42,13 +42,13 @@ void SHR16::SetLED(uint16_t led) { // Write((shr16_v & ~SHR16_LED_MSK) | led); } -void SHR16::SetTMCEnabled(uint8_t ena) { +void SHR16::SetTMCEnabled(uint8_t index, bool ena) { // ena ^= 7; // ena = ((ena & 1) << 1) | ((ena & 2) << 2) | ((ena & 4) << 3); // 0. << 1 == 1., 1. << 2 == 3., 2. << 3 == 5. // Write((shr16_v & ~SHR16_ENA_MSK) | ena); } -void SHR16::SetTMCDir(uint8_t dir) { +void SHR16::SetTMCDir(uint8_t index, bool dir) { // dir = (dir & 1) | ((dir & 2) << 1) | ((dir & 4) << 2); // 0., 1. << 1 == 2., 2. << 2 == 4. // Write((shr16_v & ~SHR16_DIR_MSK) | dir); } diff --git a/src/hal/shr16.h b/src/hal/shr16.h index 7120696..3c81b17 100644 --- a/src/hal/shr16.h +++ b/src/hal/shr16.h @@ -41,8 +41,10 @@ class SHR16 { public: void Init(); void SetLED(uint16_t led); - void SetTMCEnabled(uint8_t ena); - void SetTMCDir(uint8_t dir); + + /// indices: PULLEY = 0, SELECTOR = 1, IDLER = 2 + void SetTMCEnabled(uint8_t index, bool ena); + void SetTMCDir(uint8_t index, bool dir); private: uint16_t shr16_v; diff --git a/src/logic/mm_control.cpp b/src/logic/mm_control.cpp index 9ca0e7e..1256a32 100644 --- a/src/logic/mm_control.cpp +++ b/src/logic/mm_control.cpp @@ -2,131 +2,192 @@ #include "../modules/motion.h" #include "../modules/leds.h" #include "../modules/buttons.h" +#include "../modules/finda.h" +#include "../modules/permanent_storage.h" namespace logic { -// treba tyto zakladni male automaty poslouzi k dalsim ucelum +// "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 -// - unload to finda +// - load/unload to finda // -// ten automat musi dat vedet, ze jeste bezi nebo ze uz skoncil -// hlavni unload filament se podle toho prepne do nejakeho jineho stavu -// budou tedy 2 kroky -// - zadani maleho automatu -// - if( skoncil ) -// nastav jiny automat, updatuj progress operace atd. -// else -// cekej (pripadne updatuj progress taky) -void Logic::UnloadFilament() { - // unloads filament from extruder - filament is above Bondtech gears - modules::motion::motion.InitAxis(modules::motion::Pulley); +// 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 - // state 1 engage idler - modules::motion::motion.Idler(modules::motion::Motion::Engage); // if idler is in parked position un-park him get in contact with filament +/// 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(); } - // state 2 rotate pulley as long as the FINDA is on - if (modules::finda::active()) { - motion_unload_to_finda(); - } else { - if (checkOk()) { - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - return; + /// 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.Status() == mf::Off) { + 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; } } - // state 3 move a little bit so it is not a grinded hole in filament - modules::motion::motion.PlanMove(modules::motion::Pulley, -100, 10); // @@TODO constants - - // state 4 WTF??? FINDA is still sensing filament, let's try to unload it once again - if (modules::finda::active()) { - // try it 6 times - for (int i = 6; i > 0; i--) { - if (modules::finda::active()) { - // set_pulley_dir_push(); - modules::motion::motion.PlanMove(modules::motion::Pulley, 150, 3000); // @@TODO constants - // set_pulley_dir_pull(); - - // cancel move if FINDA switched off - - // int _steps = 4000; - // uint8_t _endstop_hit = 0; - // do { - // do_pulley_step(); - // _steps--; - // delayMicroseconds(3000); - // if (! modules::finda::active()) _endstop_hit++; - // } while (_endstop_hit < 100 && _steps > 0); + /// @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.Status() == modules::finda::Off) { + // 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; + } } - // delay(100); + 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 him 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 == UnloadToFinda2::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 } } - // state 5 error, wait for user input - if (modules::finda::active()) { - bool _continue = false; - bool _isOk = false; - - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - - modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::blink0); - - do { - if (modules::buttons::buttons.ButtonPressed(modules::buttons::Left)) { - // just move filament little bit - modules::motion::motion.Idler(modules::motion::Motion::Engage); - // set_pulley_dir_pull(); - - modules::motion::motion.PlanMove(modules::motion::Pulley, 200, 10); // @@TODO constants - // for (int i = 0; i < 200; i++) { - // do_pulley_step(); - // delayMicroseconds(5500); - // } - - // wait for move to finish - - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - } else if (modules::buttons::buttons.ButtonPressed(modules::buttons::Middle)) { - // check if everything is ok - modules::motion::motion.Idler(modules::motion::Motion::Engage); - _isOk = checkOk(); - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - } else if (modules::buttons::buttons.ButtonPressed(modules::buttons::Right)) { - // continue with unloading - modules::motion::motion.Idler(modules::motion::Motion::Engage); - _isOk = checkOk(); - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - } - if (_isOk) { - _continue = true; - } - } while (!_continue); - - //shr16_set_led(1 << 2 * (4 - active_extruder)); - modules::leds::leds.SetMode(active_extruder, modules::leds::red, modules::leds::off); - modules::leds::leds.SetMode(active_extruder, modules::leds::green, modules::leds::on); - modules::motion::motion.Idler(modules::motion::Motion::Engage); - } else { - // correct unloading - // unload to PTFE tube - // set_pulley_dir_pull(); - modules::motion::motion.PlanMove(modules::motion::Pulley, 450, 10); // @@TODO constants - // wait for move to finish - - // for (int i = 450; i > 0; i--) // 570 - // { - // do_pulley_step(); - // delayMicroseconds(5000); - // } + /// @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 } - - modules::motion::motion.Idler(modules::motion::Motion::Disengage); - - modules::motion::motion.DisableAxis(modules::motion::Pulley); - - isFilamentLoaded = false; // filament unloaded -} +}; } // namespace logic diff --git a/src/logic/mm_control.h b/src/logic/mm_control.h index 2724798..4ba47ea 100644 --- a/src/logic/mm_control.h +++ b/src/logic/mm_control.h @@ -1,12 +1,35 @@ #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 { -// schvalne zkusime udelat operaci unload filament +/// 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 { diff --git a/src/main.cpp b/src/main.cpp index 56a73fa..847bf6d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,22 +1,28 @@ #include "hal/cpu.h" #include "hal/adc.h" #include "hal/gpio.h" +#include "hal/shr16.h" #include "hal/spi.h" #include "hal/usart.h" -#include "hal/shr16.h" #include "pins.h" #include #include "modules/buttons.h" +#include "modules/finda.h" #include "modules/leds.h" #include "modules/protocol.h" #include "logic/mm_control.h" static modules::protocol::Protocol protocol; -static modules::buttons::Buttons buttons; -static modules::leds::LEDs leds; +//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; +/// remember the request message that started the currently running command +modules::protocol::RequestMsg currentCommandRq(modules::protocol::RequestMsgCodes::unknown, 0); // examples and test code shall be located here void TmpPlayground() { @@ -70,8 +76,8 @@ void setup() { cpu::Init(); shr16::shr16.Init(); - leds.SetMode(4, modules::leds::Color::green, modules::leds::Mode::blink0); - leds.Step(0); + modules::leds::leds.SetMode(4, modules::leds::Color::green, modules::leds::Mode::blink0); + modules::leds::leds.Step(0); // @@TODO if the shift register doesn't work we really can't signalize anything, only internal variables will be accessible if the UART works @@ -81,8 +87,8 @@ void setup() { .baudrate = 115200, }; hal::usart::usart1.Init(&usart_conf); - leds.SetMode(3, modules::leds::Color::green, modules::leds::Mode::on); - leds.Step(0); + modules::leds::leds.SetMode(3, modules::leds::Color::green, modules::leds::Mode::on); + modules::leds::leds.Step(0); // @@TODO if both shift register and the UART are dead, we are sitting ducks :( @@ -96,19 +102,109 @@ void setup() { .cpol = 1, }; spi::Init(SPI0, &spi_conf); - leds.SetMode(2, modules::leds::Color::green, modules::leds::Mode::on); - leds.Step(0); + modules::leds::leds.SetMode(2, modules::leds::Color::green, modules::leds::Mode::on); + modules::leds::leds.Step(0); // tmc::Init() - leds.SetMode(1, modules::leds::Color::green, modules::leds::Mode::on); - leds.Step(0); + modules::leds::leds.SetMode(1, modules::leds::Color::green, modules::leds::Mode::on); + modules::leds::leds.Step(0); // adc::Init(); - leds.SetMode(0, modules::leds::Color::green, modules::leds::Mode::on); - leds.Step(0); + modules::leds::leds.SetMode(0, modules::leds::Color::green, modules::leds::Mode::on); + modules::leds::leds.Step(0); +} + +void SendMessage(const modules::protocol::ResponseMsg &msg) { +} + +void PlanCommand(const modules::protocol::RequestMsg &rq) { + namespace mp = modules::protocol; + if ((currentCommand == nullptr) || (currentCommand->Status() == 1)) { + // we are allowed to start a new command + switch (rq.code) { + case mp::RequestMsgCodes::Cut: + //currentCommand = &cutCommand; + break; + case mp::RequestMsgCodes::Eject: + //currentCommand = &ejectCommand; + break; + case mp::RequestMsgCodes::Load: + // currentCommand = &loadCommand; + break; + case mp::RequestMsgCodes::Tool: + // currentCommand = &toolCommand; + break; + case mp::RequestMsgCodes::Unload: + // currentCommand = &unloadCommand; + break; + default: + // currentCommand = &noCommand; + break; + } + currentCommand->Reset(); + } +} + +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)); + } } void ProcessRequestMsg(const modules::protocol::RequestMsg &rq) { + namespace mp = modules::protocol; + switch (rq.code) { + case mp::RequestMsgCodes::Button: + // behave just like if the user pressed a button + break; + case mp::RequestMsgCodes::Finda: + // immediately report FINDA status + SendMessage(mp::ResponseMsg(rq, mp::ResponseMsgParamCodes::Accepted, modules::finda::finda.Status())); + break; + case mp::RequestMsgCodes::Mode: + // immediately switch to normal/stealth as requested + // modules::motion::SetMode(); + break; + case mp::RequestMsgCodes::Query: + // immediately report progress of currently running command + ReportRunningCommand(); + break; + case mp::RequestMsgCodes::Reset: + // immediately reset the board - there is no response in this case + break; // @@TODO + case mp::RequestMsgCodes::Version: + SendMessage(mp::ResponseMsg(rq, mp::ResponseMsgParamCodes::Accepted, 1)); // @@TODO + case mp::RequestMsgCodes::Wait: + break; // @@TODO + case mp::RequestMsgCodes::Cut: + case mp::RequestMsgCodes::Eject: + case mp::RequestMsgCodes::Load: + case mp::RequestMsgCodes::Tool: + case mp::RequestMsgCodes::Unload: + PlanCommand(rq); + break; + default: + // respond with an error message + break; + } } /// @returns true if a request was successfully finished @@ -124,7 +220,7 @@ bool CheckMsgs() { // just continue reading break; case mpd::Error: - // what shall we do? Start some watchdog? + // @@TODO what shall we do? Start some watchdog? We cannot send anything spontaneously break; } } @@ -149,8 +245,10 @@ void loop() { if (CheckMsgs()) { ProcessRequestMsg(protocol.GetRequestMsg()); } - buttons.Step(hal::adc::ReadADC(0)); - leds.Step(0); + modules::buttons::buttons.Step(hal::adc::ReadADC(0)); + modules::leds::leds.Step(0); + modules::finda::finda.Step(); + currentCommand->Step(); } int main() { diff --git a/src/modules/motion.cpp b/src/modules/motion.cpp index a92e906..051c6a9 100644 --- a/src/modules/motion.cpp +++ b/src/modules/motion.cpp @@ -1,4 +1,5 @@ #include "motion.h" +#include "../hal/shr16.h" namespace modules { namespace motion { @@ -9,11 +10,46 @@ void Motion::PlanMove(Axis axis, float targetPosition, uint16_t feedrate) {} void Motion::Home(Axis axis) {} -void Motion::SetMode(Mode mode) {} +void Motion::SetMode(MotorMode mode) {} void Motion::Step() {} void ISR() {} +//@@TODO check the directions +void StepDirPins::SetIdlerDirUp() { + hal::shr16::shr16.SetTMCDir(Axis::Idler, true); +} + +void StepDirPins::SetIdlerDirDown() { + hal::shr16::shr16.SetTMCDir(Axis::Idler, false); +} + +void StepDirPins::SetSelectorDirLeft() { + hal::shr16::shr16.SetTMCDir(Axis::Selector, true); +} +void StepDirPins::SetSelectorDirRight() { + hal::shr16::shr16.SetTMCDir(Axis::Selector, false); +} + +void StepDirPins::SetPulleyDirPull() { + hal::shr16::shr16.SetTMCDir(Axis::Pulley, true); +} +void StepDirPins::SetPulleyDirPush() { + hal::shr16::shr16.SetTMCDir(Axis::Pulley, false); +} + +void StepDirPins::StepIdler(uint8_t on) { + // PORTD |= idler_step_pin; +} + +void StepDirPins::StepSelector(uint8_t on) { + // PORTD |= selector_step_pin; +} + +void StepDirPins::StepPulley(uint8_t on) { + // PORTB |= pulley_step_pin; +} + } // namespace motion } // namespace modules diff --git a/src/modules/motion.h b/src/modules/motion.h index 8740398..075e4e7 100644 --- a/src/modules/motion.h +++ b/src/modules/motion.h @@ -25,16 +25,39 @@ namespace modules { namespace motion { enum Axis { - Idler, + Pulley, Selector, - Pulley + Idler, }; -enum Mode { +enum MotorMode { Stealth, Normal }; +enum IdlerMode { + Engage, + Disengage +}; + +/// As step and dir pins are randomly scattered on the board for each of the axes/motors +/// it is convenient to make a common interface for them +class StepDirPins { +public: + static void SetIdlerDirUp(); + static void SetIdlerDirDown(); + + static void SetSelectorDirLeft(); + static void SetSelectorDirRight(); + + static void SetPulleyDirPull(); + static void SetPulleyDirPush(); + + static void StepIdler(uint8_t on); + static void StepSelector(uint8_t on); + static void StepPulley(uint8_t on); +}; + class Motion { public: /// Init axis driver @@ -51,14 +74,18 @@ public: /// Set mode of TMC/motors operation /// Common for all axes/motors - void SetMode(Mode mode); + void SetMode(MotorMode mode); /// State machine doing all the planning and stepping preparation based on received commands void Step(); + /// @returns true if all planned moves have been finished + bool QueueEmpty() const; + + /// stop whatever moves are being done + void AbortPlannedMoves() {} + /// probably higher-level operations knowing the semantic meaning of axes - enum IdlerMode { Engage, - Disengage }; void Idler(IdlerMode im) {} private: