Michal
/
Prusa-Firmware-MMU
mirror of https://github.com/prusa3d/Prusa-Firmware-MMU
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Introduce Idler::PartiallyDisengage 

This is to solve a potential problem while feeding to printer's drive gears - while disengaging the Idler, the Pulley was still rotating to avoid grinding the filament (printer is pulling it).
Other filaments could have moved a bit when the Idler's bearings ran over them while the Pulley was still rotating slowly -> the filament could have been moved into the Selector's path causing trouble (especially when not used in the print).
Therefore, the Idler disengages partially now - moves into an intermediate position between the slots.
Then, the Pulley is completely stopped and after that the Idler does a full disengage like before.
pull/178/head
D.R.racer 2022-07-31 10:23:08 +02:00 committed by DRracer
parent 4964e9a781
commit 5dffd32025
10 changed files with 92 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/config/config.h
View File

@ -204,6 +204,16 @@ static constexpr U_deg idlerSlotPositions[toolCount + 1] = {
IdlerOffsetFromHome ///18.0_deg Fully disengaged all slots
};
/// Intermediate positions for Idler's slots: 0-4 are the real ones, the 5th index is the idle position
static constexpr U_deg idlerIntermediateSlotPositions[toolCount + 1] = {
IdlerOffsetFromHome + 4.5F * IdlerSlotDistance,
IdlerOffsetFromHome + 3.5F * IdlerSlotDistance,
IdlerOffsetFromHome + 2.5F * IdlerSlotDistance,
IdlerOffsetFromHome + 1.5F * IdlerSlotDistance,
IdlerOffsetFromHome + 0.5F * IdlerSlotDistance,
IdlerOffsetFromHome ///18.0_deg Fully disengaged all slots
};
static constexpr U_deg idlerParkPositionDelta = -IdlerSlotDistance + 5.0_deg / 2; ///@TODO verify
static constexpr U_deg_s idlerFeedrate = 300._deg_s;

2
src/logic/command_base.cpp
View File

@ -187,7 +187,7 @@ bool CommandBase::CheckToolIndex(uint8_t index) {
}
void CommandBase::ErrDisengagingIdler() {
if (!mi::idler.Engaged()) {
if (mi::idler.Disengaged()) {
state = ProgressCode::ERRWaitingForUser;
error = deferredErrorCode;
deferredErrorCode = ErrorCode::OK; // and clear the deferredEC just for safety

2
src/logic/eject_filament.cpp
View File

@ -66,7 +66,7 @@ bool EjectFilament::StepInner() {
}
break;
case ProgressCode::DisengagingIdler:
if (!mi::idler.Engaged()) { // idler disengaged
if (mi::idler.Disengaged()) { // idler disengaged
mpu::pulley.Disable();
mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::NotLoaded);
FinishedOK();

13
src/logic/feed_to_bondtech.cpp
View File

@ -69,18 +69,25 @@ bool FeedToBondtech::Step() {
case PushingFilamentIntoNozzle:
if (mm::motion.QueueEmpty()) {
mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InNozzle);
mi::idler.Disengage();
mi::idler.PartiallyDisengage(mg::globals.ActiveSlot());
// while disengaging the idler, keep on moving with the pulley to avoid grinding while the printer is trying to grab the filament itself
mpu::pulley.PlanMove(config::fsensorToNozzleAvoidGrind, config::pulleySlowFeedrate);
state = PartiallyDisengagingIdler;
}
return false;
case PartiallyDisengagingIdler:
if (mi::idler.PartiallyDisengaged()) {
mm::motion.AbortPlannedMoves(mm::Pulley);
mpu::pulley.Disable();
mi::idler.Disengage(); // disengage fully while Pulley is already stopped
state = DisengagingIdler;
}
return false;
case DisengagingIdler:
if (!mi::idler.Engaged()) {
if (mi::idler.Disengaged()) {
dbg_logic_P(PSTR("Feed to Bondtech --> Idler disengaged"));
dbg_logic_fP(PSTR("Pulley end steps %u"), mpu::pulley.CurrentPosition_mm());
state = OK;
mpu::pulley.Disable();
ml::leds.SetMode(mg::globals.ActiveSlot(), ml::green, ml::on);
}
return false;

1
src/logic/feed_to_bondtech.h
View File

@ -17,6 +17,7 @@ struct FeedToBondtech {
PushingFilamentFast,
PushingFilamentToFSensor,
PushingFilamentIntoNozzle,
PartiallyDisengagingIdler,
DisengagingIdler,
OK,
Failed,

4
src/logic/load_filament.cpp
View File

@ -100,8 +100,8 @@ bool LoadFilament::StepInner() {
case ProgressCode::DisengagingIdler:
// beware - this state is being reused for error recovery
// and if the selector decided to re-home, we have to wait for it as well
// therefore: 'if (!mi::idler.Engaged())' : alone is not enough
if (!mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
// therefore: 'if (mi::idler.Disengaged())' : alone is not enough
if (mi::idler.Disengaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
LoadFinishedCorrectly();
}
break;

2
src/logic/unload_filament.cpp
View File

@ -79,7 +79,7 @@ bool UnloadFilament::StepInner() {
}
return false;
case ProgressCode::DisengagingIdler:
if (!mi::idler.Engaged() && ms::selector.State() == ms::Selector::Ready) {
if (mi::idler.Disengaged() && ms::selector.State() == ms::Selector::Ready) {
UnloadFinishedCorrectly();
}
return false;

28
src/modules/idler.cpp
View File

@ -13,7 +13,9 @@ namespace idler {
Idler idler;
void Idler::PrepareMoveToPlannedSlot() {
mm::motion.PlanMoveTo<mm::Idler>(SlotPosition(plannedSlot), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
mm::motion.PlanMoveTo<mm::Idler>(
plannedMove == Operation::engage ? SlotPosition(plannedSlot) : IntermediateSlotPosition(plannedSlot),
mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
dbg_logic_fP(PSTR("Prepare Move Idler slot %d"), plannedSlot);
}
@ -40,7 +42,7 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() {
mm::motion.SetPosition(mm::Idler, 0);
// finish whatever has been planned before homing
if (!plannedEngage) {
if (plannedMove == Operation::disengage) {
plannedSlot = IdleSlotIndex();
}
InitMovement();
@ -48,8 +50,8 @@ bool Idler::FinishHomingAndPlanMoveToParkPos() {
}
void Idler::FinishMove() {
currentlyEngaged = plannedEngage;
if (!Engaged()) // turn off power into the Idler motor when disengaged (no force necessary)
currentlyEngaged = plannedMove;
if (Disengaged()) // turn off power into the Idler motor when disengaged (no force necessary)
mm::motion.Disable(mm::Idler);
}
@ -59,7 +61,7 @@ Idler::OperationResult Idler::Disengage() {
return OperationResult::Refused;
}
plannedSlot = IdleSlotIndex();
plannedEngage = false;
plannedMove = Operation::disengage;
// coordinates invalid, first home, then disengage
if (!homingValid) {
@ -68,7 +70,7 @@ Idler::OperationResult Idler::Disengage() {
}
// already disengaged
if (!Engaged()) {
if (Disengaged()) {
dbg_logic_P(PSTR("Idler Disengaged"));
return OperationResult::Accepted;
}
@ -77,14 +79,22 @@ Idler::OperationResult Idler::Disengage() {
return InitMovement();
}
Idler::OperationResult Idler::PartiallyDisengage(uint8_t slot) {
return PlanMoveInner(slot, Operation::intermediate);
}
Idler::OperationResult Idler::Engage(uint8_t slot) {
return PlanMoveInner(slot, Operation::engage);
}
Idler::OperationResult Idler::PlanMoveInner(uint8_t slot, Operation plannedOp) {
if (state == Moving) {
dbg_logic_P(PSTR("Moving --> Engage refused"));
return OperationResult::Refused;
}
plannedSlot = slot;
plannedEngage = true;
plannedMove = plannedOp;
// if we are homing right now, just record the desired planned slot and return Accepted
if (state == HomeBack) {
@ -101,12 +111,10 @@ Idler::OperationResult Idler::Engage(uint8_t slot) {
}
// already engaged
if (Engaged()) {
dbg_logic_P(PSTR("Idler Engaged"));
if (currentlyEngaged == plannedMove) {
return OperationResult::Accepted;
}
// engaging
return InitMovement();
}

33
src/modules/idler.h
View File

@ -16,8 +16,8 @@ class Idler : public motion::MovableBase {
public:
inline constexpr Idler()
: MovableBase(mm::Idler)
, plannedEngage(false)
, currentlyEngaged(false) {}
, plannedMove(Operation::disengage)
, currentlyEngaged(Operation::disengage) {}
/// Plan engaging of the idler to a specific filament slot
/// @param slot index to be activated
@ -28,20 +28,31 @@ public:
/// @returns #OperationResult
OperationResult Disengage();
/// Plan partial disengaging of the idler
/// @returns #OperationResult
OperationResult PartiallyDisengage(uint8_t slot);
/// Performs one step of the state machine according to currently planned operation
/// @returns true if the idler is ready to accept new commands (i.e. it has finished the last operation)
bool Step();
/// @returns the current state of idler - engaged / disengaged
/// @returns the current state of idler - engaged / disengaged / partially disengaged
/// this state is updated only when a planned move is successfully finished, so it is safe for higher-level
/// state machines to use this call as a waiting condition for the desired state of the idler
inline bool Engaged() const { return currentlyEngaged; }
inline bool Engaged() const { return currentlyEngaged == Operation::engage; }
inline bool Disengaged() const { return currentlyEngaged == Operation::disengage; }
inline bool PartiallyDisengaged() const { return currentlyEngaged == Operation::intermediate; }
/// @returns predefined positions of individual slots
static constexpr mm::I_pos_t SlotPosition(uint8_t slot) {
return mm::unitToAxisUnit<mm::I_pos_t>(config::idlerSlotPositions[slot]);
}
/// @returns predefined intermediate positions between individual slots
static constexpr mm::I_pos_t IntermediateSlotPosition(uint8_t slot) {
return mm::unitToAxisUnit<mm::I_pos_t>(config::idlerIntermediateSlotPositions[slot]);
}
/// @returns the index of idle position of the idler, usually 5 in case of 0-4 valid indices of filament slots
inline static constexpr uint8_t IdleSlotIndex() { return config::toolCount; }
@ -59,11 +70,19 @@ protected:
virtual void FinishMove() override;
private:
/// direction of travel - engage/disengage
bool plannedEngage;
enum class Operation : uint8_t {
disengage = 0,
engage = 1,
intermediate = 2 ///< planned movement will be to an intermediate position between slots (different array of positions)
};
OperationResult PlanMoveInner(uint8_t slot, Operation plannedOp);
/// direction of travel - engage/disengage/disengageIntermediate
Operation plannedMove;
/// current state
bool currentlyEngaged;
Operation currentlyEngaged;
};
/// The one and only instance of Idler in the FW

31
tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp
View File

@ -24,8 +24,10 @@ namespace ha = hal::adc;
TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
using namespace logic;
uint8_t slot = 0;
ForceReinitAllAutomata();
REQUIRE(EnsureActiveSlotIndex(0, mg::FilamentLoadState::AtPulley));
REQUIRE(EnsureActiveSlotIndex(slot, mg::FilamentLoadState::AtPulley));
FeedToBondtech fb;
main_loop();
@ -35,10 +37,10 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
REQUIRE(fb.State() == FeedToBondtech::EngagingIdler);
// it should have instructed the selector and idler to move to slot 0
// it should have instructed the selector and idler to move to a slot
// check if the idler and selector have the right command
CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(0).v);
CHECK(mm::AxisNearestTargetPos(mm::Selector) == ms::Selector::SlotPosition(0).v);
CHECK(mm::AxisNearestTargetPos(mm::Idler) == mi::Idler::SlotPosition(slot).v);
CHECK(mm::AxisNearestTargetPos(mm::Selector) == ms::Selector::SlotPosition(slot).v);
CHECK(mm::axes[mm::Idler].enabled == true);
// engaging idler
@ -47,8 +49,8 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
[&](uint32_t) { return !mi::idler.Engaged(); },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(slot).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v);
CHECK(mm::axes[mm::Pulley].enabled);
// idler engaged, selector in position, we'll start pushing filament
@ -81,15 +83,26 @@ TEST_CASE("feed_to_bondtech::feed_phase_unlimited", "[feed_to_bondtech]") {
[&](uint32_t) { return fb.State() == FeedToBondtech::PushingFilamentIntoNozzle; },
5000));
// disengaging idler
// partially disengaging idler
REQUIRE(fb.State() == FeedToBondtech::PartiallyDisengagingIdler);
REQUIRE(WhileCondition(
fb,
[&](uint32_t) { return fb.State() == FeedToBondtech::PartiallyDisengagingIdler; },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::IntermediateSlotPosition(slot).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v);
CHECK_FALSE(mm::axes[mm::Pulley].enabled);
// fully disengaging idler
REQUIRE(fb.State() == FeedToBondtech::DisengagingIdler);
REQUIRE(WhileCondition(
fb,
[&](uint32_t) { return fb.State() == FeedToBondtech::DisengagingIdler; },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(mi::idler.IdleSlotIndex()).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(slot).v);
CHECK_FALSE(mm::axes[mm::Pulley].enabled);
// state machine finished ok, the green LED should be on