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

Add Load filament state machine 

+ printer's fsensor (external) module
+ state machine for loading to bondtech
pull/21/head
D.R.racer 2021-06-10 10:52:13 +02:00 committed by DRracer
parent 7cab9dc915
commit dfb57bcae5
17 changed files with 368 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -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

8
src/logic/cut_filament.cpp
View File

@ -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;

2
src/logic/eject_filament.cpp
View File

@ -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;
}
}

3
src/logic/error_codes.h
View File

@ -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,
};

59
src/logic/feed_to_bondtech.cpp Normal file
View File

@ -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

40
src/logic/feed_to_bondtech.h Normal file
View File

@ -0,0 +1,40 @@
#pragma once
#include <stdint.h>
/// @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

188
src/logic/load_filament.cpp
View File

@ -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

5
src/logic/load_filament.h
View File

@ -1,7 +1,8 @@
#pragma once
#include <stdint.h>
#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;

7
src/logic/progress_codes.h
View File

@ -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,

2
src/logic/tool_change.cpp
View File

@ -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;
}
}

2
src/logic/unload_filament.cpp
View File

@ -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;
}

5
src/main.cpp
View File

@ -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();

17
src/modules/fsensor.cpp Normal file
View File

@ -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

30
src/modules/fsensor.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#include <stdint.h>
#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

4
src/modules/idler.h
View File

@ -17,9 +17,9 @@ public:
};
inline Idler()
: plannedEngage(false)
: state(Ready)
, plannedEngage(false)
, plannedSlot(0)
, state(Ready)
, currentSlot(0)
, currentlyEngaged(false) {}

6
src/modules/motion.h
View File

@ -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);

4
src/modules/selector.h
View File

@ -18,8 +18,8 @@ public:
inline Selector()
: state(Ready)
, currentSlot(0)
, plannedSlot(0) {}
, plannedSlot(0)
, currentSlot(0) {}
// public operations on the selector