diff --git a/src/modules/axisunit.h b/src/modules/axisunit.h
index 3d0eba9..736a6fc 100644
--- a/src/modules/axisunit.h
+++ b/src/modules/axisunit.h
@@ -20,26 +20,52 @@ 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.
+/// convenience methods to convert from physical units to AxisUnits directly at compile
+/// time. AxisUnits are just as efficient as non-checked pos_t and steps_t.
 ///
 /// 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
+/// 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
+///   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 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 be used at compile-time (constants, fixed-lenght moves).
+///
+/// If runtime manipulation is necessary, use AxisUnit should always be used instead.
+/// Conversion from physical to AxisUnit can be done through 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 unitToSteps instead.
 ///
-/// 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.
+/// should be avoided as it bypasses type checks. AxisUnit can also be constructed by
+/// providing a counter as the first initializer.
 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;
 
@@ -50,58 +76,81 @@ struct AxisUnit {
     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 to AxisUnit
+template <typename T, typename U>
+static constexpr T unitToAxisUnit(U v) {
+    static_assert(T::unit == U::unit, "incorrect unit type conversion");
+    static_assert(U::base == axisScale[T::axis].base, "incorrect unit base conversion");
+    return { (typename T::type_t)(v.v * axisScale[T::axis].stepsPerUnit) };
+}
+
+/// Convert an Unit to a steps type (pos_t or steps_t)
+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)
 
-/// Convert a Unit to AxisUnit
-template <typename T, typename U>
-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<P_pos_t>(config::pulley.stepsPerUnit, config::U_mm { 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::pulley.stepsPerUnit, config::U_mm { 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::pulley.stepsPerUnit, config::U_mm_s2 { 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::selector.stepsPerUnit, config::U_mm { 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::selector.stepsPerUnit, config::U_mm_s { 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::selector.stepsPerUnit, config::U_mm_s2 { 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::idler.stepsPerUnit, config::U_deg { 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::idler.stepsPerUnit, config::U_deg_s { 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::idler.stepsPerUnit, config::U_deg_s2 { deg_s2 }) };
+    return { unitToAxisUnit<I_accel_t>(config::U_deg_s2 { deg_s2 }) };
 }
 
 } // namespace motion
diff --git a/src/modules/motion.h b/src/modules/motion.h
index e33ed60..c8c377e 100644
--- a/src/modules/motion.h
+++ b/src/modules/motion.h
@@ -28,8 +28,6 @@ struct AxisParams {
     MotorParams params;
     MotorCurrents currents;
     MotorMode mode;
-    long double stepsPerUnit;
-    config::UnitBase unitBase;
     steps_t jerk;
     steps_t accel;
 };
@@ -39,12 +37,6 @@ 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 <typename AU, typename U>
-static constexpr typename AU::type_t unitToSteps(const long double stepsPerUnit, U v) {
-    return unitToAxisUnit<AU>(stepsPerUnit, v).v;
-}
-
 /// Static axis configuration
 static constexpr AxisParams axisParams[NUM_AXIS] = {
     // Pulley
@@ -53,10 +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),
-        .stepsPerUnit = config::pulley.stepsPerUnit,
-        .unitBase = config::PulleyLimits::base,
-        .jerk = unitToSteps<P_speed_t>(config::pulley.stepsPerUnit, config::pulleyLimits.jerk),
-        .accel = unitToSteps<P_accel_t>(config::pulley.stepsPerUnit, config::pulleyLimits.accel),
+        .jerk = unitToSteps<P_speed_t>(config::pulleyLimits.jerk),
+        .accel = unitToSteps<P_accel_t>(config::pulleyLimits.accel),
     },
     // Selector
     {
@@ -64,10 +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),
-        .stepsPerUnit = config::selector.stepsPerUnit,
-        .unitBase = config::SelectorLimits::base,
-        .jerk = unitToSteps<S_speed_t>(config::selector.stepsPerUnit, config::selectorLimits.jerk),
-        .accel = unitToSteps<S_accel_t>(config::selector.stepsPerUnit, config::selectorLimits.accel),
+        .jerk = unitToSteps<S_speed_t>(config::selectorLimits.jerk),
+        .accel = unitToSteps<S_accel_t>(config::selectorLimits.accel),
     },
     // Idler
     {
@@ -75,10 +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),
-        .stepsPerUnit = config::idler.stepsPerUnit,
-        .unitBase = config::IdlerLimits::base,
-        .jerk = unitToSteps<I_speed_t>(config::idler.stepsPerUnit, config::idlerLimits.jerk),
-        .accel = unitToSteps<I_accel_t>(config::idler.stepsPerUnit, config::idlerLimits.accel),
+        .jerk = unitToSteps<I_speed_t>(config::idlerLimits.jerk),
+        .accel = unitToSteps<I_accel_t>(config::idlerLimits.accel),
     },
 };
 
@@ -133,10 +119,9 @@ public:
     template <Axis A, config::UnitBase B>
     constexpr void PlanMoveTo(config::Unit<long double, B, Lenght> pos,
         config::Unit<long double, B, Speed> feedrate) {
-        static_assert(B == axisParams[A].unitBase, "incorrect unit base");
         PlanMoveTo<A>(
-            unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(axisParams[A].stepsPerUnit, pos),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(axisParams[A].stepsPerUnit, feedrate));
+            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
@@ -162,10 +147,9 @@ public:
     template <Axis A, config::UnitBase B>
     constexpr void PlanMove(config::Unit<long double, B, Lenght> delta,
         config::Unit<long double, B, Speed> feedrate) {
-        static_assert(B == axisParams[A].unitBase, "incorrect unit base");
         PlanMove<A>(
-            unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(axisParams[A].stepsPerUnit, delta),
-            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(axisParams[A].stepsPerUnit, feedrate));
+            unitToAxisUnit<AxisUnit<pos_t, A, Lenght>>(delta),
+            unitToAxisUnit<AxisUnit<steps_t, A, Speed>>(feedrate));
     }
 
     /// @returns head position of an axis (last enqueued position)
@@ -264,7 +248,7 @@ private:
 };
 
 /// ISR stepping routine
-extern void ISR();
+//extern void ISR();
 
 extern Motion motion;
 
diff --git a/src/unit.h b/src/unit.h
index f6a8f92..52525fb 100644
--- a/src/unit.h
+++ b/src/unit.h
@@ -41,6 +41,9 @@ 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;