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

Merge pull request #71 from wavexx/motion_units 

Motion units
pull/65/head
DRracer 2021-07-27 06:34:32 +02:00 committed by GitHub
parent 7f39f07679 1abc8713bb
commit 151a672f56
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
19 changed files with 519 additions and 92 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
src/config/axis.h
View File

@ -1,18 +1,44 @@
#pragma once
#include <stdint.h>
#include "../unit.h"
namespace config {
using namespace unit;
/// Axis configuration data
struct AxisConfig {
bool dirOn; ///< direction ON state (for inversion)
uint8_t uSteps; ///< microstepping [1-32]
bool vSense; ///< vSense scaling
uint8_t iRun; ///< running current
uint8_t iHold; ///< holding current
uint16_t accel; ///< Acceleration (unit/s^2)
uint16_t jerk; ///< Jerk (unit/s)
bool stealth; ///< Default to Stealth mode
uint8_t uSteps; ///< microstepping [1-256]
long double stepsPerUnit; ///< steps per unit
};
/// List of available axes
enum Axis : uint8_t {
Pulley,
Selector,
Idler,
_Axis_Last = Idler
};
/// Number of available axes
static constexpr uint8_t NUM_AXIS = Axis::_Axis_Last + 1;
/// Phisical limits for an axis
template <UnitBase B>
struct AxisLimits {
static constexpr UnitBase base = B;
Unit<long double, B, Lenght> lenght; ///< Longest move that can be performed by the axis
Unit<long double, B, Speed> jerk; ///< Maximum jerk for the axis
Unit<long double, B, Accel> accel; ///< Maximum acceleration for the axis
};
typedef AxisLimits<Millimeter> PulleyLimits; ///< Pulley axis limits
typedef AxisLimits<Millimeter> SelectorLimits; ///< Selector axis limits
typedef AxisLimits<Degree> IdlerLimits; ///< Idler axis limits
} // namespace config

71
src/config/config.h
View File

@ -8,10 +8,14 @@ namespace config {
static constexpr const uint8_t toolCount = 5U; ///< Max number of extruders/tools/slots
// Idler's setup
static constexpr uint16_t idlerSlotPositions[toolCount + 1] = { 1, 2, 3, 4, 5, 0 }; ///< slots 0-4 are the real ones, the 5th is the idle position
static constexpr U_deg idlerSlotPositions[toolCount + 1] = {
1.0_deg, 2.0_deg, 3.0_deg, 4.0_deg, 5.0_deg, 0
}; ///< slots 0-4 are the real ones, the 5th is the idle position
// Selector's setup
static constexpr uint16_t selectorSlotPositions[toolCount + 1] = { 1, 2, 3, 4, 5, 6 }; ///< slots 0-4 are the real ones, the 5th is the farthest parking positions
static constexpr U_mm selectorSlotPositions[toolCount + 1] = {
1.0_mm, 2.0_mm, 3.0_mm, 4.0_mm, 5.0_mm, 6.0_mm
}; ///< slots 0-4 are the real ones, the 5th is the farthest parking positions
// Printer's filament sensor setup
static constexpr const uint16_t fsensorDebounceMs = 10;
@ -27,11 +31,6 @@ static constexpr const uint16_t buttonsDebounceMs = 100;
static constexpr const uint16_t buttonADCLimits[buttonCount][2] = { { 0, 50 }, { 80, 100 }, { 160, 180 } };
static constexpr const uint8_t buttonsADCIndex = 5; ///< ADC index of buttons input
/// Maximum microstepping resolution. This defines the effective unit of
/// the step intevals on the motion API, independently of the selected
/// microstepping interval.
static constexpr uint8_t uStepMaxRes = 32;
/// Do not plan moves equal or shorter than the requested steps
static constexpr uint8_t dropSegments = 0;
@ -51,40 +50,58 @@ static constexpr uint8_t stepTimerFrequencyDivider = 8;
/// 16 = 8us (25us is the max frequency interval per maxStepFrequency)
static constexpr uint8_t stepTimerQuantum = 16;
/// Idler configuration
static constexpr AxisConfig idler = {
.dirOn = true,
.uSteps = 16,
.vSense = false,
.iRun = 20,
.iHold = 20,
.accel = 100,
.jerk = 10,
.stealth = false,
};
/// Pulley configuration
/// Pulley axis configuration
static constexpr AxisConfig pulley = {
.dirOn = true,
.uSteps = 16,
.vSense = false,
.iRun = 20,
.iHold = 20,
.accel = 100,
.jerk = 10,
.stealth = false,
.uSteps = 16,
.stepsPerUnit = 100,
};
/// Pulley motion limits
static constexpr PulleyLimits pulleyLimits = {
.lenght = 100.0_mm,
.jerk = 10.0_mm_s,
.accel = 1000.0_mm_s2,
};
/// Selector configuration
static constexpr AxisConfig selector = {
.dirOn = true,
.uSteps = 16,
.vSense = false,
.iRun = 20,
.iHold = 20,
.accel = 100,
.jerk = 10,
.stealth = false
.stealth = false,
.uSteps = 16,
.stepsPerUnit = 100,
};
/// Selector motion limits
static constexpr SelectorLimits selectorLimits = {
.lenght = 100.0_mm,
.jerk = 10.0_mm_s,
.accel = 1000.0_mm_s2,
};
/// Idler configuration
static constexpr AxisConfig idler = {
.dirOn = true,
.vSense = false,
.iRun = 20,
.iHold = 20,
.stealth = false,
.uSteps = 16,
.stepsPerUnit = 100,
};
/// Idler motion limits
static constexpr IdlerLimits idlerLimits = {
.lenght = 360.0_deg,
.jerk = 10.0_deg_s,
.accel = 1000.0_deg_s2,
};
} // namespace config

2
src/logic/eject_filament.cpp
View File

@ -50,7 +50,7 @@ bool EjectFilament::Step() {
if (mm::motion.QueueEmpty()) { // selector parked aside
state = ProgressCode::EjectingFilament;
mm::motion.InitAxis(mm::Pulley);
mm::motion.PlanMove(mm::Pulley, ejectSteps, 1500);
mm::motion.PlanMove<mm::Pulley>(ejectLength, ejectSpeed);
}
break;
case ProgressCode::EjectingFilament:

6
src/logic/eject_filament.h
View File

@ -2,9 +2,14 @@
#include <stdint.h>
#include "command_base.h"
#include "unload_filament.h"
#include "../modules/axisunit.h"
namespace logic {
// These cannot be class memebers without definition until c++17
static constexpr modules::motion::P_pos_t ejectLength = 50.0_P_mm; //@@TODO
static constexpr modules::motion::P_speed_t ejectSpeed = 1000.0_P_mm_s; //@@TODO
/// @brief A high-level command state machine - handles the complex logic of ejecting filament
///
/// The eject operation consists of:
@ -31,7 +36,6 @@ public:
ErrorCode Error() const override;
private:
constexpr static const uint16_t ejectSteps = 500; //@@TODO
UnloadFilament unl; ///< a high-level command/operation may be used as a building block of other operations as well
uint8_t slot;
void MoveSelectorAside();

175
src/modules/axisunit.h Normal file
View File

@ -0,0 +1,175 @@
#pragma once
#include "../config/axis.h"
#include "pulse_gen.h"
namespace modules {
namespace motion {
// Import required types
using config::Axis;
using config::Idler;
using config::Pulley;
using config::Selector;
using config::Accel;
using config::Lenght;
using config::Speed;
using pulse_gen::pos_t;
using pulse_gen::steps_t;
/// Specialized axis unit type for compile-time conformability testing. Like for
/// unit::Unit this is done ensure quantities are not mixed between types, while also
/// providing convenience methods to convert from physical units to AxisUnits directly at
/// compile time. AxisUnits are just as efficient as the non-checked pulse_gen::pos_t and
/// pulse_gen::steps_t.
///
/// Each axis provides separate types for each quantity, since the low-level count is also
/// not directly comparable across each (depending on the configuration settings).
/// Quantities are normally defined through the literal operators. Types and base axes are
/// prefixed with a single letter identifier for the axis: P=pulley, S=selector, I=idler.
///
/// P_pos_t pulley_position = 10.0_P_mm;
/// auto pulley_zero = 0.0_P_mm; // implicit type
/// P_speed_ pulley_feedrate = 30.0_P_mm_s;
/// I_pos_t idler_position = 15.0_I_deg;
/// pulley_position + idler_position; // compile time error
///
/// motion::Motion.PlanMove (and related functions) support both physical and AxisUnit
/// natively. This is done by specifying the axis through the first template parameter,
/// which ensures related units are also conforming:
///
/// motion.PlanMoveTo<Pulley>(10.0_mm, 100._mm_s); // using physical units
/// motion.PlanMoveTo<Pulley>(10.0_P_mm, 100._P_mm_s); // using AxisUnit
///
/// Physical units are always represented with the largest floating point type, so they
/// should only preferably be used at compile-time only.
///
/// If runtime manipulation is necessary, AxisUnit should be used instead. Conversion from
/// physical to AxisUnit can be done through motion::unitToAxisUnit:
///
/// unitToAxisUnit<final_type>(physical_type)
///
/// Examples:
///
/// P_pos_t pulley_pos = unitToAxisUnit<P_pos_t>(10.0_mm);
/// P_speed_t pulley_speed = unitToAxisUnit<P_speed_t>(100.0_mm_s);
///
/// Conversion to pos_t or steps_t works the same using motion::unitToSteps instead.
///
/// The low-level step count can be accessed when necessary through AxisUnit::v, which
/// should be avoided as it bypasses all type checks. AxisUnit can also be constructed
/// without checks by providing a counter as the first initializer.
///
/// The scaling factor is stored with the pair config::AxisConfig::uSteps and
/// config::AxisConfig::stepsPerUnit.
template <typename T, Axis A, config::UnitType U>
struct AxisUnit {
T v;
static constexpr Axis axis = A;
static constexpr config::UnitType unit = U;
typedef T type_t;
typedef AxisUnit<T, A, U> self_t;
constexpr self_t operator+(const self_t r) { return { v + r.v }; }
constexpr self_t operator-(const self_t r) { return { v - r.v }; }
constexpr self_t operator-() { return { -v }; }
constexpr self_t operator*(const self_t r) { return { v * r.v }; }
constexpr self_t operator/(const self_t r) { return { v / r.v }; }
};
/// Axis type conversion table for template expansion
struct AxisScale {
unit::UnitBase base;
long double stepsPerUnit;
};
static constexpr AxisScale axisScale[config::NUM_AXIS] = {
{ config::pulleyLimits.base, config::pulley.stepsPerUnit },
{ config::selectorLimits.base, config::selector.stepsPerUnit },
{ config::idlerLimits.base, config::idler.stepsPerUnit },
};
/// Convert a unit::Unit to AxisUnit.
/// The scaling factor is stored with the pair config::AxisConfig::uSteps and
/// config::AxisConfig::stepsPerUnit (one per-axis).
template <typename AU, typename U>
static constexpr AU unitToAxisUnit(U v) {
static_assert(AU::unit == U::unit, "incorrect unit type conversion");
static_assert(U::base == axisScale[AU::axis].base, "incorrect unit base conversion");
return { (typename AU::type_t)(v.v * axisScale[AU::axis].stepsPerUnit) };
}
/// Convert an unit::Unit to a steps type (pos_t or steps_t).
/// Extract the raw step count from an AxisUnit with type checking.
template <typename AU, typename U>
static constexpr typename AU::type_t unitToSteps(U v) {
return unitToAxisUnit<AU>(v).v;
}
// Pulley
typedef AxisUnit<pos_t, Pulley, Lenght> P_pos_t; ///< Pulley position type (steps)
typedef AxisUnit<steps_t, Pulley, Speed> P_speed_t; ///< Pulley speed type (steps/s)
typedef AxisUnit<steps_t, Pulley, Accel> P_accel_t; ///< Pulley acceleration type (steps/s2)
static constexpr P_pos_t operator"" _P_mm(long double mm) {
return { unitToAxisUnit<P_pos_t>(config::U_mm { mm }) };
}
static constexpr P_speed_t operator"" _P_mm_s(long double mm_s) {
return { unitToAxisUnit<P_speed_t>(config::U_mm_s { mm_s }) };
}
static constexpr P_accel_t operator"" _P_mm_s2(long double mm_s2) {
return { unitToAxisUnit<P_accel_t>(config::U_mm_s2 { mm_s2 }) };
}
// Selector
typedef AxisUnit<pos_t, Selector, Lenght> S_pos_t; ///< Selector position type (steps)
typedef AxisUnit<steps_t, Selector, Speed> S_speed_t; ///< Selector speed type (steps/s)
typedef AxisUnit<steps_t, Selector, Accel> S_accel_t; ///< Selector acceleration type (steps/s2)
static constexpr S_pos_t operator"" _S_mm(long double mm) {
return { unitToAxisUnit<S_pos_t>(config::U_mm { mm }) };
}
static constexpr S_speed_t operator"" _S_mm_s(long double mm_s) {
return { unitToAxisUnit<S_speed_t>(config::U_mm_s { mm_s }) };
}
static constexpr S_accel_t operator"" _S_mm_s2(long double mm_s2) {
return { unitToAxisUnit<S_accel_t>(config::U_mm_s2 { mm_s2 }) };
}
// Idler
typedef AxisUnit<pos_t, Idler, Lenght> I_pos_t; ///< Idler position type (steps)
typedef AxisUnit<steps_t, Idler, Speed> I_speed_t; ///< Idler speed type (steps/s)
typedef AxisUnit<steps_t, Idler, Accel> I_accel_t; ///< Idler acceleration type (steps/s2)
static constexpr I_pos_t operator"" _I_deg(long double deg) {
return { unitToAxisUnit<I_pos_t>(config::U_deg { deg }) };
}
static constexpr I_speed_t operator"" _I_deg_s(long double deg_s) {
return { unitToAxisUnit<I_speed_t>(config::U_deg_s { deg_s }) };
}
static constexpr I_accel_t operator"" _I_deg_s2(long double deg_s2) {
return { unitToAxisUnit<I_accel_t>(config::U_deg_s2 { deg_s2 }) };
}
} // namespace motion
} // namespace modules
// Inject literal operators into the global namespace for convenience
using modules::motion::operator"" _P_mm;
using modules::motion::operator"" _P_mm_s;
using modules::motion::operator"" _P_mm_s2;
using modules::motion::operator"" _S_mm;
using modules::motion::operator"" _S_mm_s;
using modules::motion::operator"" _S_mm_s2;
using modules::motion::operator"" _I_deg;
using modules::motion::operator"" _I_deg_s;
using modules::motion::operator"" _I_deg_s2;

4
src/modules/idler.cpp
View File

@ -22,7 +22,7 @@ bool Idler::Disengage() {
mm::motion.InitAxis(mm::Idler);
// plan move to idle position
mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[IdleSlotIndex()] - mm::motion.Position(mm::Idler), 1000); // @@TODO
mm::motion.PlanMoveTo<mm::Idler>(SlotPosition(IdleSlotIndex()), 1000._I_deg_s); // @@TODO
state = Moving;
return true;
}
@ -38,7 +38,7 @@ bool Idler::Engage(uint8_t slot) {
return true;
mm::motion.InitAxis(mm::Idler);
mm::motion.PlanMove(mm::Idler, config::idlerSlotPositions[slot] - mm::motion.Position(mm::Idler), 1000); // @@TODO
mm::motion.PlanMoveTo<mm::Idler>(SlotPosition(slot), 1000._I_deg_s); // @@TODO
state = Moving;
return true;
}

8
src/modules/idler.h
View File

@ -1,12 +1,14 @@
#pragma once
#include "../config/config.h"
#include <stdint.h>
#include "../modules/axisunit.h"
namespace modules {
/// The idler namespace provides all necessary facilities related to the logical model of the idler device of the MMU unit.
namespace idler {
namespace mm = modules::motion;
/// The Idler model handles asynchronnous Engaging / Disengaging operations and keeps track of idler's current state.
class Idler {
public:
@ -50,7 +52,9 @@ public:
inline uint8_t Slot() const { return currentSlot; }
/// @returns predefined positions of individual slots
inline static uint16_t SlotPosition(uint8_t slot) { return config::idlerSlotPositions[slot]; }
static constexpr mm::I_pos_t SlotPosition(uint8_t slot) {
return mm::unitToAxisUnit<mm::I_pos_t>(config::idlerSlotPositions[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; }

83
src/modules/motion.h
View File

@ -1,6 +1,7 @@
#pragma once
#include "../pins.h"
#include "pulse_gen.h"
#include "axisunit.h"
namespace modules {
@ -9,10 +10,11 @@ namespace modules {
/// Ideally enable stepping of motors under ISR (all timers have higher priority than serial)
namespace motion {
// Import axes definitions
using config::NUM_AXIS;
using namespace hal::tmc2130;
using pulse_gen::pos_t;
using pulse_gen::st_timer_t;
using pulse_gen::steps_t;
// Check for configuration invariants
static_assert(
@ -21,15 +23,6 @@ static_assert(
"stepTimerQuantum must be smaller than the maximal stepping frequency interval");
/// Main axis enumeration
enum Axis : uint8_t {
Pulley,
Selector,
Idler,
_Axis_Last = Idler
};
static constexpr uint8_t NUM_AXIS = _Axis_Last + 1;
struct AxisParams {
char name;
MotorParams params;
@ -52,8 +45,8 @@ static constexpr AxisParams axisParams[NUM_AXIS] = {
.params = { .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .uSteps = config::pulley.uSteps },
.currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold },
.mode = DefaultMotorMode(config::pulley),
.jerk = config::pulley.jerk,
.accel = config::pulley.accel,
.jerk = unitToSteps<P_speed_t>(config::pulleyLimits.jerk),
.accel = unitToSteps<P_accel_t>(config::pulleyLimits.accel),
},
// Selector
{
@ -61,8 +54,8 @@ static constexpr AxisParams axisParams[NUM_AXIS] = {
.params = { .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .uSteps = config::selector.uSteps },
.currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold },
.mode = DefaultMotorMode(config::selector),
.jerk = config::selector.jerk,
.accel = config::selector.accel,
.jerk = unitToSteps<S_speed_t>(config::selectorLimits.jerk),
.accel = unitToSteps<S_accel_t>(config::selectorLimits.accel),
},
// Idler
{
@ -70,8 +63,8 @@ static constexpr AxisParams axisParams[NUM_AXIS] = {
.params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps },
.currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold },
.mode = DefaultMotorMode(config::idler),
.jerk = config::idler.jerk,
.accel = config::idler.accel,
.jerk = unitToSteps<I_speed_t>(config::idlerLimits.jerk),
.accel = unitToSteps<I_accel_t>(config::idlerLimits.accel),
},
};
@ -112,6 +105,25 @@ public:
/// @param feedrate maximum feedrate
void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate);
/// Enqueue a single axis move using PlanMoveTo, but using AxisUnit. The Axis needs to
/// be supplied as the first template argument: PlanMoveTo<axis>(pos, rate).
/// @see PlanMoveTo, unitToSteps
template <Axis A>
constexpr void PlanMoveTo(AxisUnit<pos_t, A, Lenght> pos, AxisUnit<steps_t, A, Speed> feedrate) {
PlanMoveTo(A, pos.v, feedrate.v);
}
/// Enqueue a single axis move using PlanMoveTo, but using physical units. The Axis
/// needs to be supplied as the first template argument: PlanMoveTo<axis>(pos, rate).
/// @see PlanMoveTo, unitToSteps
template <Axis A, config::UnitBase B>
constexpr void PlanMoveTo(config::Unit<long double, B, Lenght> pos,
config::Unit<long double, B, Speed> feedrate) {
PlanMoveTo<A>(
unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(pos),
unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
}
/// Enqueue a single axis move in steps starting and ending at zero speed with maximum
/// feedrate. Moves can only be enqueued if the axis is not Full().
/// @param axis axis affected
@ -121,6 +133,25 @@ public:
PlanMoveTo(axis, Position(axis) + delta, feedrate);
}
/// Enqueue a single axis move using PlanMove, but using AxisUnit. The Axis needs to
/// be supplied as the first template argument: PlanMove<axis>(pos, rate).
/// @see PlanMove, unitToSteps
template <Axis A>
constexpr void PlanMove(AxisUnit<pos_t, A, Lenght> delta, AxisUnit<steps_t, A, Speed> feedrate) {
PlanMove(A, delta.v, feedrate.v);
}
/// Enqueue a single axis move using PlanMove, but using physical units. The Axis needs to
/// be supplied as the first template argument: PlanMove<axis>(pos, rate).
/// @see PlanMove, unitToSteps
template <Axis A, config::UnitBase B>
constexpr void PlanMove(config::Unit<long double, B, Lenght> delta,
config::Unit<long double, B, Speed> feedrate) {
PlanMove<A>(
unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta),
unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
}
/// @returns head position of an axis (last enqueued position)
/// @param axis axis affected
pos_t Position(Axis axis) const;
@ -140,7 +171,7 @@ public:
/// Get current acceleration for the selected axis
/// @param axis axis affected
/// @returns acceleration
steps_t Acceleration(Axis axis) {
steps_t Acceleration(Axis axis) const {
return axisData[axis].ctrl.Acceleration();
}
@ -151,6 +182,20 @@ public:
axisData[axis].ctrl.SetAcceleration(accel);
}
/// Get current jerk for the selected axis
/// @param axis axis affected
/// @returns jerk
steps_t Jerk(Axis axis) const {
return axisData[axis].ctrl.Jerk();
}
/// Set maximum jerk for the selected axis
/// @param axis axis affected
/// @param max_jerk maximum jerk
void SetJerk(Axis axis, steps_t max_jerk) {
return axisData[axis].ctrl.SetJerk(max_jerk);
}
/// State machine doing all the planning and stepping. Called by the stepping ISR.
/// @returns the interval for the next tick
st_timer_t Step();
@ -204,7 +249,7 @@ private:
};
/// ISR stepping routine
extern void Isr();
//extern void Isr();
extern Motion motion;

6
src/modules/pulse_gen.h
View File

@ -24,6 +24,12 @@ class PulseGen {
public:
PulseGen(steps_t max_jerk, steps_t acceleration);
/// @returns the jerk for the axis
steps_t Jerk() const { return max_jerk; };
/// Set maximum jerk for the axis
void SetJerk(steps_t max_jerk) { this->max_jerk = max_jerk; };
/// @returns the acceleration for the axis
steps_t Acceleration() const { return acceleration; };

2
src/modules/selector.cpp
View File

@ -21,7 +21,7 @@ bool Selector::MoveToSlot(uint8_t slot) {
return true;
mm::motion.InitAxis(mm::Selector);
mm::motion.PlanMove(mm::Selector, config::selectorSlotPositions[slot] - mm::motion.Position(mm::Selector), 1000); // @@TODO
mm::motion.PlanMoveTo<mm::Selector>(SlotPosition(slot), 1000.0_S_mm_s); // @@TODO
state = Moving;
return true;
}

8
src/modules/selector.h
View File

@ -1,12 +1,14 @@
#pragma once
#include "../config/config.h"
#include <stdint.h>
#include "../modules/axisunit.h"
namespace modules {
/// The selector namespace provides all necessary facilities related to the logical model of the selector device of the MMU unit.
namespace selector {
namespace mm = modules::motion;
/// The selector model - handles asynchronnous move operations between filament individual slots and keeps track of selector's current state.
class Selector {
public:
@ -41,7 +43,9 @@ public:
inline uint8_t Slot() const { return currentSlot; }
/// @returns predefined positions of individual slots
inline static uint16_t SlotPosition(uint8_t slot) { return config::selectorSlotPositions[slot]; }
static constexpr mm::S_pos_t SlotPosition(uint8_t slot) {
return mm::unitToAxisUnit<mm::S_pos_t>(config::selectorSlotPositions[slot]);
}
/// @returns the index of idle position of the selector, usually 5 in case of 0-4 valid indices of filament slots
inline static constexpr uint8_t IdleSlotIndex() { return config::toolCount; }

99
src/unit.h Normal file
View File

@ -0,0 +1,99 @@
#pragma once
#include <stdint.h>
/// Introduce a minimal Unit class that can be used for conformability, type checking and
/// conversion at compile time. Template parameters are abused to create unique types,
/// which then can go through (explicit) overload and conversion. Despite looking
/// daunting, usage is quite straightforward once the appropriate aliases and inline
/// operators are defined:
///
/// U_mm distance = 10.0_mm;
/// auto another = 20.5_mm;
/// auto sum = distance + another;
///
/// auto angle = 15.0_deg;
/// auto test = distance + angle; // compile time error
///
/// Template parameters are only used for type checking. Unit contains a single value
/// Unit<T>::v and is thus well suited for parameter passing and inline initialization.
///
/// Conversion to physical steps is done in modules::motion through the sister class
/// modules::motion::AxisUnit, modules::motion::unitToAxisUnit and
/// modules::motion::unitToSteps, which also ensures quantities from different axes are
/// not mixed together. AxisUnit are the normal type that *should* be used at runtime.
namespace unit {
/// Base units for conformability testing
enum UnitBase : uint8_t {
Millimeter,
Degree,
};
/// Unit types for conformability testing
enum UnitType : uint8_t {
Lenght,
Speed,
Accel,
};
/// Generic unit type for compile-time conformability testing
template <typename T, UnitBase B, UnitType U>
struct Unit {
T v;
static constexpr UnitBase base = B;
static constexpr UnitType unit = U;
typedef T type_t;
typedef Unit<T, B, U> self_t;
constexpr self_t operator+(const self_t r) { return { v + r.v }; }
constexpr self_t operator-(const self_t r) { return { v - r.v }; }
constexpr self_t operator-() { return { -v }; }
constexpr self_t operator*(const self_t r) { return { v * r.v }; }
constexpr self_t operator/(const self_t r) { return { v / r.v }; }
};
// Millimiters
typedef Unit<long double, Millimeter, Lenght> U_mm;
typedef Unit<long double, Millimeter, Speed> U_mm_s;
typedef Unit<long double, Millimeter, Accel> U_mm_s2;
static constexpr U_mm operator"" _mm(long double mm) {
return { mm };
}
static constexpr U_mm_s operator"" _mm_s(long double mm_s) {
return { mm_s };
}
static constexpr U_mm_s2 operator"" _mm_s2(long double mm_s2) {
return { mm_s2 };
}
// Degrees
typedef Unit<long double, Degree, Lenght> U_deg;
typedef Unit<long double, Degree, Speed> U_deg_s;
typedef Unit<long double, Degree, Accel> U_deg_s2;
static constexpr U_deg operator"" _deg(long double deg) {
return { deg };
}
static constexpr U_deg_s operator"" _deg_s(long double deg_s) {
return { deg_s };
}
static constexpr U_deg_s2 operator"" _deg_s2(long double deg_s2) {
return { deg_s2 };
}
} // namespace unit
// Inject literal operators into the global namespace for convenience
using unit::operator"" _mm;
using unit::operator"" _mm_s;
using unit::operator"" _mm_s2;
using unit::operator"" _deg;
using unit::operator"" _deg_s;
using unit::operator"" _deg_s2;

4
tests/unit/logic/eject_filament/test_eject_filament.cpp
View File

@ -41,8 +41,8 @@ TEST_CASE("eject_filament::eject0", "[eject_filament][.]") {
// it should have instructed the selector and idler to move to slot 1
// check if the idler and selector have the right command
CHECK(modules::motion::axes[modules::motion::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(modules::motion::axes[modules::motion::Selector].targetPos == ms::Selector::SlotPosition(4));
CHECK(modules::motion::axes[modules::motion::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(modules::motion::axes[modules::motion::Selector].targetPos == ms::Selector::SlotPosition(4).v);
// now cycle at most some number of cycles (to be determined yet) and then verify, that the idler and selector reached their target positions
REQUIRE(WhileTopState(ef, ProgressCode::SelectingFilamentSlot, 5000));

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

@ -44,8 +44,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
// it should have instructed the selector and idler to move to slot 0
// check if the idler and selector have the right command
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].enabled == true);
// engaging idler
@ -54,8 +54,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
[&](int) { return !mi::idler.Engaged(); },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
// idler engaged, selector in position, we'll start pushing filament
REQUIRE(fb.State() == FeedToBondtech::PushingFilament);
@ -81,7 +81,7 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
// 5000));
// CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(5)); // @@TODO constants
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
// state machine finished ok, the green LED should be on
REQUIRE(fb.State() == FeedToBondtech::OK);

20
tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp
View File

@ -44,8 +44,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
// it should have instructed the selector and idler to move to slot 1
// check if the idler and selector have the right command
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].enabled == true);
// engaging idler
@ -54,8 +54,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
[&](int) { return !mi::idler.Engaged(); },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
// idler engaged, selector in position, we'll start pushing filament
REQUIRE(ff.State() == FeedToFinda::PushingFilament);
@ -86,8 +86,8 @@ TEST_CASE("feed_to_finda::feed_phase_unlimited", "[feed_to_finda]") {
// [&](int) { return mi::idler.Engaged(); },
// 5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0)); // @@TODO constants
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v); // @@TODO constants
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
// state machine finished ok, the green LED should be on
REQUIRE(ff.State() == FeedToFinda::OK);
@ -111,8 +111,8 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
// it should have instructed the selector and idler to move to slot 1
// check if the idler and selector have the right command
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
// engaging idler
REQUIRE(WhileCondition(
@ -120,8 +120,8 @@ TEST_CASE("feed_to_finda::FINDA_failed", "[feed_to_finda]") {
[&](int) { return !mi::idler.Engaged(); },
5000));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
// idler engaged, we'll start pushing filament
REQUIRE(ff.State() == FeedToFinda::PushingFilament);

8
tests/unit/logic/helpers/helpers.ipp
View File

@ -3,9 +3,9 @@ template<typename SM>
bool VerifyState(SM &uf, bool filamentLoaded, uint8_t idlerSlotIndex, uint8_t selectorSlotIndex,
bool findaPressed, ml::Mode greenLEDMode, ml::Mode redLEDMode, ErrorCode err, ProgressCode topLevelProgress) {
CHECKED_ELSE(mg::globals.FilamentLoaded() == filamentLoaded) { return false; }
CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex)) { return false; }
CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex).v) { return false; }
CHECKED_ELSE(mi::idler.Engaged() == (idlerSlotIndex < config::toolCount)) { return false; }
CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex)) { return false; }
CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex).v) { return false; }
CHECKED_ELSE(ms::selector.Slot() == selectorSlotIndex) { return false; }
CHECKED_ELSE(mf::finda.Pressed() == findaPressed) { return false; }
@ -30,9 +30,9 @@ template<typename SM>
bool VerifyState2(SM &uf, bool filamentLoaded, uint8_t idlerSlotIndex, uint8_t selectorSlotIndex,
bool findaPressed, uint8_t ledCheckIndex, ml::Mode greenLEDMode, ml::Mode redLEDMode, ErrorCode err, ProgressCode topLevelProgress) {
CHECKED_ELSE(mg::globals.FilamentLoaded() == filamentLoaded) { return false; }
CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex)) { return false; }
CHECKED_ELSE(mm::axes[mm::Idler].pos == mi::Idler::SlotPosition(idlerSlotIndex).v) { return false; }
CHECKED_ELSE(mi::idler.Engaged() == (idlerSlotIndex < config::toolCount)) { return false; }
CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex)) { return false; }
CHECKED_ELSE(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(selectorSlotIndex).v) { return false; }
CHECKED_ELSE(ms::selector.Slot() == selectorSlotIndex) { return false; }
CHECKED_ELSE(mf::finda.Pressed() == findaPressed) { return false; }

4
tests/unit/logic/stubs/stub_motion.cpp
View File

@ -70,9 +70,11 @@ void Motion::AbortPlannedMoves() {
}
void ReinitMotion() {
const pos_t selector_pos = unitToSteps<S_pos_t>(config::selectorSlotPositions[0]);
// reset the simulation data to defaults
axes[0] = AxisSim({ 0, 0, false, false, false }); // pulley
axes[1] = AxisSim({ 1, 1, false, false, false }); // selector //@@TODO proper selector positions once defined
axes[1] = AxisSim({ selector_pos, selector_pos, false, false, false }); // selector //@@TODO proper selector positions once defined
axes[2] = AxisSim({ 0, 0, false, false, false }); // idler
}

8
tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp
View File

@ -44,8 +44,8 @@ TEST_CASE("unload_to_finda::regular_unload", "[unload_to_finda]") {
// it should have instructed the selector and idler to move to slot 1
// check if the idler and selector have the right command
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].enabled == true);
// engaging idler
@ -93,8 +93,8 @@ TEST_CASE("unload_to_finda::unload_without_FINDA_trigger", "[unload_to_finda]")
// it should have instructed the selector and idler to move to slot 1
// check if the idler and selector have the right command
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0));
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
CHECK(mm::axes[mm::Idler].targetPos == mi::Idler::SlotPosition(0).v);
CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
CHECK(mm::axes[mm::Idler].enabled == true);
// engaging idler

61
tests/unit/modules/motion/test_motion.cpp
View File

@ -31,14 +31,59 @@ TEST_CASE("motion::basic", "[motion]") {
REQUIRE(motion.Position(Idler) == 10);
}
TEST_CASE("motion::dual_move_fwd", "[motion]") {
// check for configuration values that we cannot change but should match for this test
// to function as expected (maybe this should be a static_assert?)
REQUIRE(config::idler.jerk == config::selector.jerk);
TEST_CASE("motion::unit", "[motion]") {
// test AxisUnit conversion in the PlanMove and PlanMoveTo.
using config::operator"" _mm;
using config::operator"" _mm_s;
using config::operator"" _deg;
using config::operator"" _deg_s;
REQUIRE(motion.QueueEmpty());
REQUIRE(motion.Position(Pulley) == 0);
// move with AxisUnit
pos_t target = config::pulley.stepsPerUnit * 10;
motion.PlanMoveTo<Pulley>(10.0_P_mm, 100.0_P_mm_s);
CHECK(stepUntilDone());
REQUIRE(motion.Position(Pulley) == target);
// move directly with physical units
motion.PlanMoveTo<Pulley>(10.0_mm, 100.0_mm_s);
REQUIRE(stepUntilDone() == 0);
REQUIRE(motion.Position(Pulley) == target);
// relative move with AxisUnit
motion.PlanMove<Pulley>(-5.0_P_mm, 100.0_P_mm_s);
CHECK(stepUntilDone());
REQUIRE(motion.Position(Pulley) == target / 2);
// relative move with physical unit
motion.PlanMove<Pulley>(-5.0_mm, 100.0_mm_s);
CHECK(stepUntilDone());
REQUIRE(motion.Position(Pulley) == 0);
// now test remaining axes
target = config::selector.stepsPerUnit * 10;
motion.PlanMoveTo<Selector>(10.0_S_mm, 100.0_S_mm_s);
motion.PlanMove<Selector>(10.0_mm, 100.0_mm_s);
CHECK(stepUntilDone());
REQUIRE(motion.Position(Selector) == target * 2);
target = config::idler.stepsPerUnit * 10;
motion.PlanMoveTo<Idler>(10.0_I_deg, 100.0_I_deg_s);
motion.PlanMove<Idler>(10.0_deg, 100.0_deg_s);
CHECK(stepUntilDone());
REQUIRE(motion.Position(Idler) == target * 2);
}
TEST_CASE("motion::dual_move_fwd", "[motion]") {
// enqueue moves on two axes
REQUIRE(motion.QueueEmpty());
// ensure the same jerk is set on both
motion.SetJerk(Idler, motion.Jerk(Selector));
REQUIRE(motion.Jerk(Idler) == motion.Jerk(Selector));
// ensure the same acceleration is set on both
motion.SetAcceleration(Idler, motion.Acceleration(Selector));
REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector));
@ -57,13 +102,13 @@ TEST_CASE("motion::dual_move_fwd", "[motion]") {
}
TEST_CASE("motion::dual_move_inv", "[motion]") {
// check for configuration values that we cannot change but should match for this test
// to function as expected (maybe this should be a static_assert?)
REQUIRE(config::idler.jerk == config::selector.jerk);
// enqueue moves on two axes
REQUIRE(motion.QueueEmpty());
// ensure the same jerk is set on both
motion.SetJerk(Idler, motion.Jerk(Selector));
REQUIRE(motion.Jerk(Idler) == motion.Jerk(Selector));
// ensure the same acceleration is set on both
motion.SetAcceleration(Idler, motion.Acceleration(Selector));
REQUIRE(motion.Acceleration(Idler) == motion.Acceleration(Selector));