Merge pull request #71 from wavexx/motion_units

Motion units

src/config/axis.h

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

src/config/config.h

@@ -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
+/// Pulley axis configuration
-static constexpr AxisConfig idler = {
-    .dirOn = true,
-    .uSteps = 16,
-    .vSense = false,
-    .iRun = 20,
-    .iHold = 20,
-    .accel = 100,
-    .jerk = 10,
-    .stealth = false,
-};
-
-/// Pulley 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,
+    .stealth = false,
-    .jerk = 10,
+    .uSteps = 16,
-    .stealth = false
+    .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

src/logic/eject_filament.cpp

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

src/logic/eject_filament.h

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

src/modules/axisunit.h

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

src/modules/idler.cpp

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

src/modules/idler.h

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

src/modules/motion.h

@@ -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,
+        .jerk = unitToSteps<P_speed_t>(config::pulleyLimits.jerk),
-        .accel = config::pulley.accel,
+        .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,
+        .jerk = unitToSteps<S_speed_t>(config::selectorLimits.jerk),
-        .accel = config::selector.accel,
+        .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,
+        .jerk = unitToSteps<I_speed_t>(config::idlerLimits.jerk),
-        .accel = config::idler.accel,
+        .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;
 

src/modules/pulse_gen.h

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

src/modules/selector.cpp

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

src/modules/selector.h

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

src/unit.h

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

tests/unit/logic/eject_filament/test_eject_filament.cpp

@@ -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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(modules::motion::axes[modules::motion::Selector].targetPos == ms::Selector::SlotPosition(4));
+    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));

tests/unit/logic/feed_to_bondtech/test_feed_to_bondtech.cpp

@@ -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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    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::Idler].pos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+    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);

tests/unit/logic/feed_to_finda/test_feed_to_finda.cpp

@@ -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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    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::Idler].pos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+    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::Idler].pos == mi::Idler::SlotPosition(0).v); // @@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(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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    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::Idler].pos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].pos == ms::Selector::SlotPosition(0).v);
 
     // idler engaged, we'll start pushing filament
     REQUIRE(ff.State() == FeedToFinda::PushingFilament);

tests/unit/logic/helpers/helpers.ipp

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

tests/unit/logic/stubs/stub_motion.cpp

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

tests/unit/logic/unload_to_finda/test_unload_to_finda.cpp

@@ -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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    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::Idler].targetPos == mi::Idler::SlotPosition(0).v);
-    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0));
+    CHECK(mm::axes[mm::Selector].targetPos == ms::Selector::SlotPosition(0).v);
     CHECK(mm::axes[mm::Idler].enabled == true);
 
     // engaging idler

tests/unit/modules/motion/test_motion.cpp

@@ -31,14 +31,59 @@ TEST_CASE("motion::basic", "[motion]") {
     REQUIRE(motion.Position(Idler) == 10);
 }
 
-TEST_CASE("motion::dual_move_fwd", "[motion]") {
+TEST_CASE("motion::unit", "[motion]") {
-    // check for configuration values that we cannot change but should match for this test
+    // test AxisUnit conversion in the PlanMove and PlanMoveTo.
-    // to function as expected (maybe this should be a static_assert?)
+    using config::operator"" _mm;
-    REQUIRE(config::idler.jerk == config::selector.jerk);
+    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));