From 0d3bf52543907a7577e04026e31c31956d8ece19 Mon Sep 17 00:00:00 2001 From: "D.R.racer" Date: Fri, 25 Nov 2022 10:38:32 +0100 Subject: [PATCH] Fix and tune CutFilament + unit tests Several issues addressed in this PR: - CutFilament tuning + error recovery - introduce register 0x1d (cut filament selector iRun current level) - optimize setting iRun and iHold currents in the FW - CutFilament unit test fixed --- src/config/config.h | 2 + src/logic/cut_filament.cpp | 102 +++++++++++++----- src/logic/cut_filament.h | 4 + src/logic/load_filament.cpp | 2 +- src/modules/globals.cpp | 2 + src/modules/globals.h | 7 ++ src/modules/idler.cpp | 6 +- src/modules/movable_base.cpp | 7 ++ src/modules/movable_base.h | 4 + src/registers.cpp | 7 +- .../logic/cut_filament/test_cut_filament.cpp | 48 +++++---- tests/unit/logic/helpers/helpers.ipp | 3 +- 12 files changed, 145 insertions(+), 49 deletions(-) diff --git a/src/config/config.h b/src/config/config.h index fc58329..e0c5141 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -134,6 +134,8 @@ static constexpr AxisConfig selector = { .sg_thrs = 3, }; +static constexpr uint8_t selectorCutIRun = 40; ///< 660mA + /// Selector motion limits static constexpr SelectorLimits selectorLimits = { .lenght = 75.0_mm, // @@TODO how does this relate to SelectorOffsetFromMin? diff --git a/src/logic/cut_filament.cpp b/src/logic/cut_filament.cpp index 59edd92..1ad6278 100644 --- a/src/logic/cut_filament.cpp +++ b/src/logic/cut_filament.cpp @@ -9,6 +9,7 @@ #include "../modules/permanent_storage.h" #include "../modules/pulley.h" #include "../modules/selector.h" +#include "../modules/user_input.h" namespace logic { @@ -34,10 +35,16 @@ bool CutFilament::Reset(uint8_t param) { void CutFilament::SelectFilamentSlot() { state = ProgressCode::SelectingFilamentSlot; mi::idler.Engage(cutSlot); - ms::selector.MoveToSlot(cutSlot); + MoveSelector(cutSlot); ml::leds.SetPairButOffOthers(cutSlot, ml::blink0, ml::off); } +void CutFilament::MoveSelector(uint8_t slot) { + if (ms::selector.MoveToSlot(slot) != ms::Selector::OperationResult::Accepted) { + GoToErrDisengagingIdler(ErrorCode::FINDA_FLICKERS); + } +} + bool CutFilament::StepInner() { switch (state) { case ProgressCode::UnloadingFilament: @@ -51,55 +58,102 @@ bool CutFilament::StepInner() { case ProgressCode::SelectingFilamentSlot: if (mi::idler.Engaged() && ms::selector.Slot() == cutSlot) { // idler and selector finished their moves mg::globals.SetFilamentLoaded(cutSlot, mg::FilamentLoadState::AtPulley); - feed.Reset(true, 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 + if (feed.Reset(true, true)) { + state = ProgressCode::FeedingToFinda; + error = ErrorCode::RUNNING; } else { - // unload back to the pulley - state = ProgressCode::UnloadingToPulley; - mpu::pulley.PlanMove(-config::cutLength, mg::globals.PulleyUnloadFeedrate_mm_s()); + // selector refused to move - FINDA problem suspected + GoToErrDisengagingIdler(ErrorCode::FINDA_FLICKERS); } } break; - case ProgressCode::UnloadingToPulley: - if (mm::motion.QueueEmpty()) { // idler and selector finished their moves - // move selector aside - prepare the blade into active position - state = ProgressCode::PreparingBlade; - mg::globals.SetFilamentLoaded(cutSlot, mg::FilamentLoadState::AtPulley); - ms::selector.MoveToSlot(cutSlot + 1); + case ProgressCode::FeedingToFinda: + if (feed.Step()) { + if (feed.State() == FeedToFinda::Failed) { + GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_ON); // signal loading error + } else { + // unload back to the pulley + state = ProgressCode::RetractingFromFinda; + retract.Reset(); + } } + break; + case ProgressCode::RetractingFromFinda: + if (retract.Step()) { + if (retract.State() == RetractFromFinda::Failed) { + GoToErrDisengagingIdler(ErrorCode::FINDA_DIDNT_SWITCH_OFF); // signal loading error + } else { + // move selector aside - prepare the blade into active position + state = ProgressCode::PreparingBlade; + mg::globals.SetFilamentLoaded(cutSlot, mg::FilamentLoadState::AtPulley); + ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::blink0, ml::off); + MoveSelector(cutSlot + 1); + } + } + break; case ProgressCode::PreparingBlade: if (ms::selector.Slot() == cutSlot + 1) { state = ProgressCode::PushingFilament; - mpu::pulley.PlanMove(config::cutLength, mg::globals.PulleyUnloadFeedrate_mm_s()); // + mpu::pulley.PlanMove(config::cutLength + config::cuttingEdgeRetract, config::pulleySlowFeedrate); } break; case ProgressCode::PushingFilament: if (mm::motion.QueueEmpty()) { + state = ProgressCode::DisengagingIdler; + mi::idler.Disengage(); // disengage before doing a cut because we need extra power into the Selector motor + } + break; + case ProgressCode::DisengagingIdler: + if (mi::idler.Disengaged()) { state = ProgressCode::PerformingCut; - ms::selector.MoveToSlot(0); + // set highest available current for the Selector + ms::selector.SetCurrents(mg::globals.CutIRunCurrent(), config::selector.iHold); + // lower move speed + savedSelectorFeedRate_mm_s = mg::globals.SelectorFeedrate_mm_s().v; + mg::globals.SetSelectorFeedrate_mm_s(config::selectorHomingFeedrate.v); + MoveSelector(cutSlot); // let it cut :) } break; case ProgressCode::PerformingCut: - if (ms::selector.Slot() == 0) { // this may not be necessary if we want the selector and pulley move at once + if (ms::selector.Slot() == cutSlot) { // this may not be necessary if we want the selector and pulley move at once state = ProgressCode::ReturningSelector; - ms::selector.MoveToSlot(5); // move selector to the other end of its axis to cleanup + // revert current to Selector's normal value + ms::selector.SetCurrents(config::selector.iRun, config::selector.iHold); + // revert move speed + mg::globals.SetSelectorFeedrate_mm_s(savedSelectorFeedRate_mm_s); + ms::selector.InvalidateHoming(); + mpu::pulley.PlanMove(-config::cuttingEdgeRetract, config::pulleySlowFeedrate); } break; case ProgressCode::ReturningSelector: - if (ms::selector.Slot() == 5) { // selector returned to position, feed the filament back to FINDA + if (ms::selector.HomingValid()) { // selector rehomed FinishedOK(); ml::leds.SetPairButOffOthers(mg::globals.ActiveSlot(), ml::on, ml::off); - feed.Reset(true, true); } break; case ProgressCode::OK: return true; + case ProgressCode::ERRDisengagingIdler: + ErrDisengagingIdler(); + return false; + case ProgressCode::ERRWaitingForUser: { + // waiting for user buttons and/or a command from the printer + mui::Event ev = mui::userInput.ConsumeEvent(); + switch (ev) { + case mui::Event::Middle: // try again the whole sequence + InvalidateHoming(); + Reset(cutSlot); + break; + default: // no event, continue waiting for user input + break; + } + return false; + } + case ProgressCode::ERREngagingIdler: + if (mi::idler.Engaged()) { + state = ProgressCode::ERRHelpingFilament; + } + return false; default: // we got into an unhandled state, better report it state = ProgressCode::ERRInternal; error = ErrorCode::INTERNAL; diff --git a/src/logic/cut_filament.h b/src/logic/cut_filament.h index 1e69575..a1f332d 100644 --- a/src/logic/cut_filament.h +++ b/src/logic/cut_filament.h @@ -4,6 +4,7 @@ #include "command_base.h" #include "unload_filament.h" #include "feed_to_finda.h" +#include "retract_from_finda.h" namespace logic { @@ -29,9 +30,12 @@ private: constexpr static const uint16_t cutStepsPost = 150; UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well FeedToFinda feed; + RetractFromFinda retract; uint8_t cutSlot; + uint16_t savedSelectorFeedRate_mm_s; void SelectFilamentSlot(); + void MoveSelector(uint8_t slot); }; /// The one and only instance of CutFilament state machine in the FW diff --git a/src/logic/load_filament.cpp b/src/logic/load_filament.cpp index 3f099d2..4af0860 100644 --- a/src/logic/load_filament.cpp +++ b/src/logic/load_filament.cpp @@ -80,7 +80,7 @@ bool LoadFilament::StepInner() { // as requested in MMU-116 - stopping an unsuccessful feed should retract as well but not check the filament result = ResultCode::Cancelled; verifyLoadedFilament = 0; - // [[fallthrough]] + [[fallthrough]]; case FeedToFinda::OK: GoToRetractingFromFinda(); break; diff --git a/src/modules/globals.cpp b/src/modules/globals.cpp index d4b64e7..dfcad74 100644 --- a/src/modules/globals.cpp +++ b/src/modules/globals.cpp @@ -32,6 +32,8 @@ void Globals::Init() { ResetSelectorFeedrate(); ResetIdlerFeedrate(); + + ResetCutIRunCurrent(); } uint8_t Globals::ActiveSlot() const { diff --git a/src/modules/globals.h b/src/modules/globals.h index 93eaea6..1ae758b 100644 --- a/src/modules/globals.h +++ b/src/modules/globals.h @@ -95,6 +95,11 @@ public: /// Stores the new StallGuard threshold for an axis into EEPROM (does not affect the current state of TMC drivers at all) void SetStallGuardThreshold(config::Axis axis, uint8_t sgthrs); + /// @returns Cut iRun current level (value for TMC2130) + uint8_t CutIRunCurrent() const { return cutIRunCurrent; } + void ResetCutIRunCurrent() { cutIRunCurrent = config::selectorCutIRun; } + void SetCutIRunCurrent(uint8_t v) { cutIRunCurrent = v; } + private: /// Sets the active slot, usually after some command/operation. /// Also updates the EEPROM records accordingly @@ -114,6 +119,8 @@ private: uint16_t selectorFeedrate_mm_s; uint16_t idlerFeedrate_deg_s; + + uint8_t cutIRunCurrent; }; /// The one and only instance of global state variables diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp index 3da1444..69fd872 100644 --- a/src/modules/idler.cpp +++ b/src/modules/idler.cpp @@ -54,11 +54,9 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() { void Idler::FinishMove() { currentlyEngaged = plannedMove; if (Disengaged()) // reduce power into the Idler motor when disengaged (less force necessary) - mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, mm::axisParams[axis].currents); + SetCurrents(config::idler.iRun, config::idler.iHold); else if (Engaged()) { // maximum motor power when the idler is engaged - hal::tmc2130::MotorCurrents tempCurrent = mm::axisParams[axis].currents; - tempCurrent.iHold = tempCurrent.iRun; - mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, tempCurrent); + SetCurrents(config::idler.iRun, config::idler.iRun); } } diff --git a/src/modules/movable_base.cpp b/src/modules/movable_base.cpp index 752fd77..a26a48a 100644 --- a/src/modules/movable_base.cpp +++ b/src/modules/movable_base.cpp @@ -23,6 +23,13 @@ void MovableBase::PlanHome() { PlanHomingMoveForward(); } +void MovableBase::SetCurrents(uint8_t iRun, uint8_t iHold) { + hal::tmc2130::MotorCurrents tempCurrent = mm::axisParams[axis].currents; + tempCurrent.iRun = iRun; + tempCurrent.iHold = iHold; + mm::motion.DriverForAxis(axis).SetCurrents(mm::axisParams[axis].params, tempCurrent); +} + MovableBase::OperationResult MovableBase::InitMovement() { if (motion.InitAxis(axis)) { return InitMovementNoReinitAxis(); diff --git a/src/modules/movable_base.h b/src/modules/movable_base.h index 8e4c91f..4ab27a4 100644 --- a/src/modules/movable_base.h +++ b/src/modules/movable_base.h @@ -68,6 +68,10 @@ public: inline config::Axis Axis() const { return axis; } + /// Set TMC2130 iRun current level for this axis + /// iRun == 0 means set the default from config + void __attribute__((noinline)) SetCurrents(uint8_t iRun, uint8_t iHold); + #ifndef UNITTEST protected: #endif diff --git a/src/registers.cpp b/src/registers.cpp index 5548442..68ad82a 100644 --- a/src/registers.cpp +++ b/src/registers.cpp @@ -158,6 +158,7 @@ | 0x1ah 26 | uint16 | Get Pulley position | 0000h 0 | ffffh 65535 | unit mm | Read only | M707 A0x1a | N/A | 0x1bh 27 | uint16 | Set/Get_Selector_slot | 0000h 0 | ffffh 65535 | unit slot [0-4/5] 5=park pos | Read / Write | M707 A0x1b | M708 A0x1b Xn | 0x1ch 28 | uint16 | Set/Get_Idler_slot | 0000h 0 | ffffh 65535 | unit slot [0-4/5] 5=disengaged | Read / Write | M707 A0x1c | M708 A0x1c Xn +| 0x1dh 29 | uint8 | Set/Get Selector cut iRun current | 0 to 63 (aka 0-1024mA)| 31 (530mA) | | Read / Write | M707 A0x1d | M708 A0x1d Xn */ struct RegisterFlags { @@ -368,7 +369,11 @@ static const RegisterRec registers[] /*PROGMEM*/ = { []() -> uint16_t { return mi::idler.Slot(); }, [](uint16_t d) { d >= config::toolCount ? mi::idler.Disengage() : mi::idler.Engage(d); }, 1), - + // 0x1d Set/Get Selector cut iRun current level RW + RegisterRec( + []() -> uint16_t { return mg::globals.CutIRunCurrent(); }, + [](uint16_t d) { mg::globals.SetCutIRunCurrent(d); }, + 1), }; static constexpr uint8_t registersSize = sizeof(registers) / sizeof(RegisterRec); diff --git a/tests/unit/logic/cut_filament/test_cut_filament.cpp b/tests/unit/logic/cut_filament/test_cut_filament.cpp index 8aec809..0495199 100644 --- a/tests/unit/logic/cut_filament/test_cut_filament.cpp +++ b/tests/unit/logic/cut_filament/test_cut_filament.cpp @@ -15,6 +15,7 @@ #include "../../modules/stubs/stub_adc.h" +#include "../stubs/homing.h" #include "../stubs/main_loop_stub.h" #include "../stubs/stub_motion.h" @@ -22,18 +23,18 @@ using Catch::Matchers::Equals; #include "../helpers/helpers.ipp" -void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) { +void CutSlot(logic::CutFilament &cf, uint8_t startSlot, uint8_t cutSlot) { ForceReinitAllAutomata(); - REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley)); + REQUIRE(EnsureActiveSlotIndex(startSlot, mg::FilamentLoadState::AtPulley)); - REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, ml::off, ml::off)); + REQUIRE(VerifyEnvironmentState(mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), startSlot, false, false, ml::off, ml::off)); // restart the automaton cf.Reset(cutSlot); // check initial conditions - REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), 0, false, false, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::SelectingFilamentSlot)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::Idler::IdleSlotIndex(), startSlot, false, false, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::SelectingFilamentSlot)); // now cycle at most some number of cycles (to be determined yet) and then verify, that the idler and selector reached their target positions // Beware - with the real positions of the selector, the number of steps needed to finish some states grows, so the ~40K steps here has a reason @@ -52,7 +53,7 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) { return cf.TopLevelState() == ProgressCode::FeedingToFinda; }, 5000)); // filament fed to FINDA - REQUIRE(VerifyState2(cf, mg::FilamentLoadState::InSelector, cutSlot, cutSlot, true, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::UnloadingToPulley)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::InSelector, cutSlot, cutSlot, true, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::RetractingFromFinda)); // pull it back to the pulley + simulate FINDA depress REQUIRE(WhileCondition( @@ -61,7 +62,7 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) { if( step == 100 ){ // simulate FINDA trigger - will get depressed in 100 steps hal::gpio::WritePin(FINDA_PIN, hal::gpio::Level::low); } - return cf.TopLevelState() == ProgressCode::UnloadingToPulley; }, 5000)); + return cf.TopLevelState() == ProgressCode::RetractingFromFinda; }, 5000)); REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PreparingBlade)); @@ -71,21 +72,28 @@ void CutSlot(logic::CutFilament &cf, uint8_t cutSlot) { // pushing filament a bit for a cut REQUIRE(WhileTopState(cf, ProgressCode::PushingFilament, 5000)); - REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PerformingCut)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::DisengagingIdler)); + // disengage idler (to save power for the selector) + REQUIRE(WhileTopState(cf, ProgressCode::DisengagingIdler, idlerEngageDisengageMaxSteps)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), cutSlot + 1, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::PerformingCut)); // cutting REQUIRE(WhileTopState(cf, ProgressCode::PerformingCut, selectorMoveMaxSteps)); - REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, 0, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::ReturningSelector)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), cutSlot, false, true, cutSlot, ml::blink0, ml::off, ErrorCode::RUNNING, ProgressCode::ReturningSelector)); + + // rehoming selector + SimulateSelectorHoming(cf); - // moving selector to the other end of its axis REQUIRE(WhileTopState(cf, ProgressCode::ReturningSelector, selectorMoveMaxSteps)); - REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, cutSlot, ms::Selector::IdleSlotIndex(), false, true, cutSlot, ml::blink0, ml::off, ErrorCode::OK, ProgressCode::OK)); + REQUIRE(VerifyState2(cf, mg::FilamentLoadState::AtPulley, mi::idler.IdleSlotIndex(), ms::Selector::IdleSlotIndex(), false, true, cutSlot, ml::on, ml::off, ErrorCode::OK, ProgressCode::OK)); } TEST_CASE("cut_filament::cut0", "[cut_filament]") { - for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { - logic::CutFilament cf; - CutSlot(cf, cutSlot); + for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) { + for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { + logic::CutFilament cf; + CutSlot(cf, startSlot, cutSlot); + } } } @@ -102,12 +110,16 @@ TEST_CASE("cut_filament::state_machine_reusal", "[cut_filament]") { InvalidSlot(cf, activeSlot, config::toolCount); } - for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { - CutSlot(cf, cutSlot); + for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) { + for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { + CutSlot(cf, startSlot, cutSlot); + } } - for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { - CutSlot(cf, cutSlot); - InvalidSlot(cf, cutSlot, config::toolCount); + for (uint8_t startSlot = 0; startSlot < config::toolCount; ++startSlot) { + for (uint8_t cutSlot = 0; cutSlot < config::toolCount; ++cutSlot) { + CutSlot(cf, startSlot, cutSlot); + InvalidSlot(cf, cutSlot, config::toolCount); + } } } diff --git a/tests/unit/logic/helpers/helpers.ipp b/tests/unit/logic/helpers/helpers.ipp index 30334e6..0760f78 100644 --- a/tests/unit/logic/helpers/helpers.ipp +++ b/tests/unit/logic/helpers/helpers.ipp @@ -122,7 +122,8 @@ bool VerifyState(SM &uf, mg::FilamentLoadState fls, uint8_t idlerSlotIndex, uint CHECKED_ELSE(uf.Error() == err) { return false; } - CHECKED_ELSE(uf.TopLevelState() == topLevelProgress) { + auto tls = uf.TopLevelState(); + CHECKED_ELSE(tls == topLevelProgress) { return false; } return true;