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

Prototype of Unload Filament command/operation 

+ related stuff:
- command decoding+processing+reporting in main.cpp
- slight refactoring of modules
pull/19/head
D.R.racer 2021-06-02 11:18:49 +02:00
parent d86b3ca95a
commit 91411e0aff
7 changed files with 383 additions and 136 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/hal/avr/shr16.cpp
View File

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

6
src/hal/shr16.h
View File

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

259
src/logic/mm_control.cpp
View File

@ -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();
/// 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 {
if (checkOk()) {
modules::motion::motion.Idler(modules::motion::Motion::Disengage);
return;
// 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);
}
// delay(100);
/// @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;
}
}
return false;
case OK:
case Failed:
default:
return true;
}
}
};
// state 5 error, wait for user input
if (modules::finda::active()) {
bool _continue = false;
bool _isOk = false;
/// 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
};
modules::motion::motion.Idler(modules::motion::Motion::Disengage);
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);
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);
} else {
state = DisengagingIdler;
}
if (_isOk) {
_continue = true;
// in all cases disengage the idler
mm::motion.Idler(mm::Disengage);
}
} while (!_continue);
//shr16_set_led(1 << 2 * (4 - active_extruder));
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);
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);
// }
mm::motion.PlanMove(mm::Pulley, 450, 5000); // @@TODO constants
state = AvoidingGrind;
}
modules::motion::motion.Idler(modules::motion::Motion::Disengage);
modules::motion::motion.DisableAxis(modules::motion::Pulley);
return false;
case OK:
isFilamentLoaded = false; // filament unloaded
return true; // successfully finished
}
}
/// @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
}
};
} // namespace logic

25
src/logic/mm_control.h
View File

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

130
src/main.cpp
View File

@ -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 <avr/interrupt.h>
#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() {

38
src/modules/motion.cpp
View File

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

39
src/modules/motion.h
View File

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