From 888cdf7cd5af4857c3418c1626ff84f80f2a1853 Mon Sep 17 00:00:00 2001 From: Yuri D'Elia Date: Sun, 25 Jul 2021 16:39:54 +0200 Subject: [PATCH] Introduce compile-time axis unit type checks and conversions Introduces: - config::Unit: base class for physical quantities - motion::AxisUnit: type-checked steps type "config/unit.h" defines basic physical quantities, which are not normally used elsewhere besides config.h. "modules/axisunit.h" extends the modules::motion namespace with Axis-aware units, with one type per axis per unit. P_pos_t defines step positions for the pulley, I_pos_t for the idler, etc. These are defined through the literar operators which are similarly named and automatically convert a physical quantity to an AxisUnit at compile time: P_pos_t pulley_pos = 10.0_P_mm; Besides type-checking, AxisUnit are otherwise identical to raw step counts and are intended to be used along with the updated Motion API. PlanMove/PlanMoveTo has been extended to support moves using these units or physical quantities. Again, conversion is performed at compile time. --- src/config/axis.h | 30 +++++- src/config/config.h | 63 +++++++----- src/config/unit.h | 89 ++++++++++++++++ src/modules/axisunit.h | 119 ++++++++++++++++++++++ src/modules/motion.h | 81 +++++++++++---- tests/unit/modules/motion/test_motion.cpp | 45 ++++++++ 6 files changed, 382 insertions(+), 45 deletions(-) create mode 100644 src/config/unit.h create mode 100644 src/modules/axisunit.h diff --git a/src/config/axis.h b/src/config/axis.h index 6ca96a8..6d5acd0 100644 --- a/src/config/axis.h +++ b/src/config/axis.h @@ -1,18 +1,42 @@ #pragma once #include +#include "unit.h" namespace config { /// 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 +struct AxisLimits { + static constexpr UnitBase base = B; + Unit lenght; ///< Longest move that can be performed by the axis + Unit jerk; ///< Maximum jerk for the axis + Unit accel; ///< Maximum acceleration for the axis +}; + +typedef AxisLimits PulleyLimits; ///< Pulley axis limits +typedef AxisLimits SelectorLimits; ///< Selector axis limits +typedef AxisLimits IdlerLimits; ///< Idler axis limits + } // namespace config diff --git a/src/config/config.h b/src/config/config.h index 3875d94..ece5230 100644 --- a/src/config/config.h +++ b/src/config/config.h @@ -27,11 +27,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 +46,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 diff --git a/src/config/unit.h b/src/config/unit.h new file mode 100644 index 0000000..c855934 --- /dev/null +++ b/src/config/unit.h @@ -0,0 +1,89 @@ +#pragma once +#include + +// In this header we 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. The Unit contains a single value +// Unit::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 +// AxisUnit, which also ensures quantities from different axes are not mixed together. +// AxisUnit are the normal units that should be used at runtime, which is why physical +// units and operators are not exported into the global namespace by default. + +namespace config { + +/// 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 +struct Unit { + T v; + + typedef T type_t; + typedef Unit 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 U_mm; +typedef Unit U_mm_s; +typedef Unit 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 U_deg; +typedef Unit U_deg_s; +typedef Unit 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 config diff --git a/src/modules/axisunit.h b/src/modules/axisunit.h new file mode 100644 index 0000000..3d0eba9 --- /dev/null +++ b/src/modules/axisunit.h @@ -0,0 +1,119 @@ +#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 config::Unit +/// this is done ensure unit quantities are not mixed between types, while also providing +/// convenience methods to convert from physical units to AxisUnits directly at compile. +/// +/// Each axis unit type is separate for each axis, since the low-level count is not +/// directly comparable across axes. Quantities are normally defined through the +/// literar 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 +/// +/// modules::motion::Motion.PlanMove (and related functions) support AxisUnit natively. +/// The low-level step count can be accessed when necessary through AxisUnit::v, which +/// should be avoided as it bypasses type checks. +template +struct AxisUnit { + T v; + + typedef T type_t; + typedef AxisUnit 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 }; } +}; + +typedef AxisUnit P_pos_t; ///< Pulley position type (steps) +typedef AxisUnit P_speed_t; ///< Pulley speed type (steps/s) +typedef AxisUnit P_accel_t; ///< Pulley acceleration type (steps/s2) + +/// Convert a Unit to AxisUnit +template +static constexpr T unitToAxisUnit(const long double stepsPerUnit, U v) { + return { (typename T::type_t)(v.v * stepsPerUnit) }; +} + +static constexpr P_pos_t operator"" _P_mm(long double mm) { + return { unitToAxisUnit(config::pulley.stepsPerUnit, config::U_mm { mm }) }; +} + +static constexpr P_speed_t operator"" _P_mm_s(long double mm_s) { + return { unitToAxisUnit(config::pulley.stepsPerUnit, config::U_mm { mm_s }) }; +} + +static constexpr P_accel_t operator"" _P_mm_s2(long double mm_s2) { + return { unitToAxisUnit(config::pulley.stepsPerUnit, config::U_mm_s2 { mm_s2 }) }; +} + +typedef AxisUnit S_pos_t; ///< Selector position type (steps) +typedef AxisUnit S_speed_t; ///< Selector speed type (steps/s) +typedef AxisUnit S_accel_t; ///< Selector acceleration type (steps/s2) + +static constexpr S_pos_t operator"" _S_mm(long double mm) { + return { unitToAxisUnit(config::selector.stepsPerUnit, config::U_mm { mm }) }; +} + +static constexpr S_speed_t operator"" _S_mm_s(long double mm_s) { + return { unitToAxisUnit(config::selector.stepsPerUnit, config::U_mm_s { mm_s }) }; +} + +static constexpr S_accel_t operator"" _S_mm_s2(long double mm_s2) { + return { unitToAxisUnit(config::selector.stepsPerUnit, config::U_mm_s2 { mm_s2 }) }; +} + +typedef AxisUnit I_pos_t; ///< Idler position type (steps) +typedef AxisUnit I_speed_t; ///< Idler speed type (steps/s) +typedef AxisUnit I_accel_t; ///< Idler acceleration type (steps/s2) + +static constexpr I_pos_t operator"" _I_deg(long double deg) { + return { unitToAxisUnit(config::idler.stepsPerUnit, config::U_deg { deg }) }; +} + +static constexpr I_speed_t operator"" _I_deg_s(long double deg_s) { + return { unitToAxisUnit(config::idler.stepsPerUnit, config::U_deg_s { deg_s }) }; +} + +static constexpr I_accel_t operator"" _I_deg_s2(long double deg_s2) { + return { unitToAxisUnit(config::idler.stepsPerUnit, 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; diff --git a/src/modules/motion.h b/src/modules/motion.h index 98cc23b..109bc96 100644 --- a/src/modules/motion.h +++ b/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,20 +23,13 @@ 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; MotorCurrents currents; MotorMode mode; + long double stepsPerUnit; + config::UnitBase unitBase; steps_t jerk; steps_t accel; }; @@ -44,6 +39,12 @@ static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) { return axis.stealth ? MotorMode::Stealth : MotorMode::Normal; } +/// Convert an AxisUnit to a steps type (pos_t or steps_t) +template +static constexpr typename AU::type_t unitToSteps(const long double stepsPerUnit, U v) { + return unitToAxisUnit(stepsPerUnit, v).v; +} + /// Static axis configuration static constexpr AxisParams axisParams[NUM_AXIS] = { // Pulley @@ -52,8 +53,10 @@ 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, + .stepsPerUnit = config::pulley.stepsPerUnit, + .unitBase = config::PulleyLimits::base, + .jerk = unitToSteps(config::pulley.stepsPerUnit, config::pulleyLimits.jerk), + .accel = unitToSteps(config::pulley.stepsPerUnit, config::pulleyLimits.accel), }, // Selector { @@ -61,8 +64,10 @@ 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, + .stepsPerUnit = config::selector.stepsPerUnit, + .unitBase = config::SelectorLimits::base, + .jerk = unitToSteps(config::selector.stepsPerUnit, config::selectorLimits.jerk), + .accel = unitToSteps(config::selector.stepsPerUnit, config::selectorLimits.accel), }, // Idler { @@ -70,8 +75,10 @@ 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, + .stepsPerUnit = config::idler.stepsPerUnit, + .unitBase = config::IdlerLimits::base, + .jerk = unitToSteps(config::idler.stepsPerUnit, config::idlerLimits.jerk), + .accel = unitToSteps(config::idler.stepsPerUnit, config::idlerLimits.accel), }, }; @@ -112,6 +119,26 @@ 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(pos, rate). + /// @see PlanMoveTo, unitToSteps + template + void PlanMoveTo(AxisUnit pos, AxisUnit 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(pos, rate). + /// @see PlanMoveTo, unitToSteps + template + void PlanMoveTo(config::Unit pos, + config::Unit feedrate) { + static_assert(B == axisParams[A].unitBase, "incorrect unit base"); + PlanMoveTo( + unitToAxisUnit>(axisParams[A].stepsPerUnit, pos), + unitToAxisUnit>(axisParams[A].stepsPerUnit, 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 +148,26 @@ 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(pos, rate). + /// @see PlanMove, unitToSteps + template + void PlanMove(AxisUnit delta, AxisUnit 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(pos, rate). + /// @see PlanMove, unitToSteps + template + void PlanMove(config::Unit delta, + config::Unit feedrate) { + static_assert(B == axisParams[A].unitBase, "incorrect unit base"); + PlanMove( + unitToAxisUnit>(axisParams[A].stepsPerUnit, delta), + unitToAxisUnit>(axisParams[A].stepsPerUnit, feedrate)); + } + /// @returns head position of an axis (last enqueued position) /// @param axis axis affected pos_t Position(Axis axis) const; diff --git a/tests/unit/modules/motion/test_motion.cpp b/tests/unit/modules/motion/test_motion.cpp index f7d8d14..426fa7d 100644 --- a/tests/unit/modules/motion/test_motion.cpp +++ b/tests/unit/modules/motion/test_motion.cpp @@ -31,6 +31,51 @@ TEST_CASE("motion::basic", "[motion]") { REQUIRE(motion.Position(Idler) == 10); } +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(10.0_P_mm, 100.0_P_mm_s); + CHECK(stepUntilDone()); + REQUIRE(motion.Position(Pulley) == target); + + // move directly with physical units + motion.PlanMoveTo(10.0_mm, 100.0_mm_s); + REQUIRE(stepUntilDone() == 0); + REQUIRE(motion.Position(Pulley) == target); + + // relative move with AxisUnit + motion.PlanMove(-5.0_P_mm, 100.0_P_mm_s); + CHECK(stepUntilDone()); + REQUIRE(motion.Position(Pulley) == target / 2); + + // relative move with physical unit + motion.PlanMove(-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(10.0_S_mm, 100.0_S_mm_s); + motion.PlanMove(10.0_mm, 100.0_mm_s); + CHECK(stepUntilDone()); + REQUIRE(motion.Position(Selector) == target * 2); + + target = config::idler.stepsPerUnit * 10; + motion.PlanMoveTo(10.0_I_deg, 100.0_I_deg_s); + motion.PlanMove(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());