Revised WIP for the Motion API

- Remove the combined PlanMove(a,b,c,rate) call. If we allow the units
  of the various motors to be changed at compile time, the unit of
  rate can vary between axes.
- Build PlanMove on top of the absolute PlanMoveTo.
- Add required stubs for TMC2130.
- Allow each axis mode to be set independently, since we have this
  feature for free anyway.
- Rework internals to use PulseGen data types and structs.

CMakeLists.txt

@@ -195,6 +195,7 @@ target_sources(
           src/hal/avr/usart.cpp
           src/hal/avr/shr16.cpp
           src/hal/avr/eeprom.cpp
+          src/hal/avr/tmc2130.cpp
           src/hal/adc.cpp
           src/modules/protocol.cpp
           src/modules/buttons.cpp
@@ -205,6 +206,7 @@ target_sources(
           src/modules/idler.cpp
           src/modules/leds.cpp
           src/modules/motion.cpp
+          src/modules/pulse_gen.cpp
           src/modules/permanent_storage.cpp
           src/modules/selector.cpp
           src/modules/timebase.cpp

src/config/axis.h

@@ -10,9 +10,9 @@ struct AxisConfig {
     bool vSense; ///< vSense scaling
     uint8_t iRun; ///< running current
     uint8_t iHold; ///< holding current
-    float scale; ///< Scaling unit (unit/uStepsMaxRes)
+    uint16_t accel; ///< Acceleration (unit/s^2)
-    float accel; ///< Acceleration (unit/s^2)
+    uint16_t jerk; ///< Jerk (unit/s)
-    float jerk; ///< Jerk (unit/s)
+    bool stealth; ///< Default to Stealth mode
 };
 
 } // namespace config

src/config/config.h

@@ -51,9 +51,9 @@ static constexpr AxisConfig idler = {
     .vSense = false,
     .iRun = 20,
     .iHold = 20,
-    .scale = 1.,
+    .accel = 100,
-    .accel = 100.,
+    .jerk = 10,
-    .jerk = 1.,
+    .stealth = false,
 };
 
 /// Pulley configuration
@@ -63,9 +63,9 @@ static constexpr AxisConfig pulley = {
     .vSense = false,
     .iRun = 20,
     .iHold = 20,
-    .scale = 1.,
+    .accel = 100,
-    .accel = 100.,
+    .jerk = 10,
-    .jerk = 1.,
+    .stealth = false,
 };
 
 /// Selector configuration
@@ -75,9 +75,9 @@ static constexpr AxisConfig selector = {
     .vSense = false,
     .iRun = 20,
     .iHold = 20,
-    .scale = 1.,
+    .accel = 100,
-    .accel = 100.,
+    .jerk = 1,
-    .jerk = 1.,
+    .stealth = false
 };
 
 } // namespace config

src/hal/avr/tmc2130.cpp

@@ -0,0 +1,13 @@
+#include "../tmc2130.h"
+
+namespace hal {
+namespace tmc2130 {
+
+TMC2130::TMC2130(const MotorParams &params,
+    const MotorCurrents &currents,
+    MotorMode mode) {
+    // TODO
+}
+
+} // namespace tmc2130
+} // namespace hal

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(ejectSteps, 0, 0, 1500, 0, 0);
+            mm::motion.PlanMove(mm::Pulley, ejectSteps, 1500);
         }
         break;
     case ProgressCode::EjectingFilament:

src/logic/feed_to_bondtech.cpp

@@ -30,7 +30,7 @@ bool FeedToBondtech::Step() {
         if (mi::idler.Engaged()) {
             state = PushingFilament;
             ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
-            mm::motion.PlanMove(steps, 0, 0, 4500, 0, 0); //@@TODO constants - there was some strange acceleration sequence in the original FW,
+            mm::motion.PlanMove(mm::Pulley, steps, 4500); //@@TODO constants - there was some strange acceleration sequence in the original FW,
             // we can probably hand over some array of constants for hand-tuned acceleration + leverage some smoothing in the stepper as well
         }
         return false;

src/logic/feed_to_finda.cpp

@@ -32,7 +32,7 @@ bool FeedToFinda::Step() {
         if (mi::idler.Engaged() && ms::selector.Slot() == mg::globals.ActiveSlot()) {
             state = PushingFilament;
             ml::leds.SetMode(mg::globals.ActiveSlot(), ml::Color::green, ml::blink0);
-            mm::motion.PlanMove(feedPhaseLimited ? 1500 : 32767, 0, 0, 4000, 0, 0); //@@TODO constants
+            mm::motion.PlanMove(mm::Pulley, feedPhaseLimited ? 1500 : 32767, 4000); //@@TODO constants
             mu::userInput.Clear(); // remove all buffered events if any just before we wait for some input
         }
         return false;
@@ -41,7 +41,7 @@ bool FeedToFinda::Step() {
             mm::motion.AbortPlannedMoves(); // stop pushing filament
             // FINDA triggered - that means it works and detected the filament tip
             state = UnloadBackToPTFE;
-            mm::motion.PlanMove(-600, 0, 0, 4000, 0, 0); //@@TODO constants
+            mm::motion.PlanMove(mm::Pulley, -600, 4000); //@@TODO constants
         } else if (mm::motion.QueueEmpty()) { // all moves have been finished and FINDA didn't switch on
             state = Failed;
             // @@TODO - shall we disengage the idler?

src/modules/motion.cpp

@@ -13,22 +13,28 @@ bool Motion::StallGuard(Axis axis) { return false; }
 
 void Motion::ClearStallGuardFlag(Axis axis) {}
 
-void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {}
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {}
 
-void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {}
+pos_t Motion::CurrentPos(Axis axis) const { return axisData[axis].ctrl.Position(); }
-
-uint16_t Motion::CurrentPos(Axis axis) const { return 0; }
 
 void Motion::Home(Axis axis, bool direction) {}
 
-void Motion::SetMode(hal::tmc2130::MotorMode mode) {}
+void Motion::SetMode(Axis axis, MotorMode mode) {}
+
+bool Motion::QueueEmpty() const {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        if (!axisData[i].ctrl.QueueEmpty())
+            return false;
+    return true;
+}
+
+void Motion::AbortPlannedMoves() {
+    for (uint8_t i = 0; i != NUM_AXIS; ++i)
+        axisData[i].ctrl.AbortPlannedMoves();
+}
 
 void Motion::Step() {}
 
-bool Motion::QueueEmpty() const { return false; }
-
-void Motion::AbortPlannedMoves() {}
-
 void ISR() {}
 
 } // namespace motion

src/modules/motion.h

@@ -1,32 +1,18 @@
 #pragma once
-#include <stdint.h>
-#include "../config/config.h"
-#include "../hal/tmc2130.h"
 #include "../pins.h"
+#include "pulse_gen.h"
 
 namespace modules {
 
 /// @@TODO
 /// Logic of motor handling
 /// Ideally enable stepping of motors under ISR (all timers have higher priority than serial)
-
-/// input:
-/// motor, direction, speed (step rate), may be acceleration if necessary (not sure)
-/// enable/disable motor current
-/// stealth/normal
-
-/// Motors:
-/// idler
-/// selector
-/// pulley
-
-/// Operations:
-/// setDir();
-/// rotate(speed)
-/// rotate(speed, angle/steps)
-/// home?
 namespace motion {
 
+using namespace hal::tmc2130;
+using pulse_gen::pos_t;
+using pulse_gen::steps_t;
+
 /// Main axis enumeration
 enum Axis : uint8_t {
     Pulley,
@@ -37,28 +23,29 @@ enum Axis : uint8_t {
 
 static constexpr uint8_t NUM_AXIS = _Axis_Last + 1;
 
-/// Static axis configuration
 struct AxisParams {
     char name;
-    hal::tmc2130::MotorParams params;
+    MotorParams params;
-    hal::tmc2130::MotorCurrents currents;
+    MotorCurrents currents;
-    float scale;
+    MotorMode mode;
-    float accel;
+    steps_t jerk;
+    steps_t accel;
 };
 
+/// Return the default motor mode for an Axis
+static constexpr MotorMode DefaultMotorMode(const config::AxisConfig &axis) {
+    return axis.stealth ? MotorMode::Stealth : MotorMode::Normal;
+}
+
+/// Static axis configuration
 static constexpr AxisParams axisParams[NUM_AXIS] = {
     // Idler
     {
         .name = 'I',
-        .params = {
+        .params = { .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .uSteps = config::idler.uSteps },
-            .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 },
-        .scale = config::idler.scale,
+        .mode = DefaultMotorMode(config::idler),
+        .jerk = config::idler.jerk,
         .accel = config::idler.accel,
     },
     // Pulley
@@ -66,7 +53,8 @@ static constexpr AxisParams axisParams[NUM_AXIS] = {
         .name = 'P',
         .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 },
-        .scale = config::pulley.scale,
+        .mode = DefaultMotorMode(config::pulley),
+        .jerk = config::pulley.jerk,
         .accel = config::pulley.accel,
     },
     // Selector
@@ -74,22 +62,19 @@ static constexpr AxisParams axisParams[NUM_AXIS] = {
         .name = 'S',
         .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 },
-        .scale = config::selector.scale,
+        .mode = DefaultMotorMode(config::selector),
+        .jerk = config::selector.jerk,
         .accel = config::selector.accel,
     },
 };
 
-enum IdlerMode : uint8_t {
-    Engage,
-    Disengage
-};
-
 class Motion {
 public:
     inline constexpr Motion() = default;
 
-    /// Init axis driver - @@TODO this should be probably hidden somewhere deeper ... something should manage the axes and their state
+    /// Init axis driver - @@TODO this should be probably hidden
-    /// especially when the TMC may get randomly reset (deinited)
+    /// somewhere deeper ... something should manage the axes and their
+    /// state especially when the TMC may get randomly reset (deinited)
     void InitAxis(Axis axis);
 
     /// Disable axis motor
@@ -101,27 +86,36 @@ public:
     /// clear stall guard flag reported on an axis
     void ClearStallGuardFlag(Axis axis);
 
-    /// Enqueue move of a specific motor/axis into planner buffer
+    /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate
-    /// @param pulley, idler, selector - target coords
+    /// @param axis axis affected
-    void PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed);
+    /// @param pos target position
+    /// @param feedrate maximum feedrate
+    void PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate);
 
     /// Enqueue a single axis move in steps starting and ending at zero speed with maximum feedrate
     /// @param axis axis affected
-    /// @param delta number of steps in either direction
+    /// @param delta relative to current position
-    /// @param feedrate maximum feedrate/speed after acceleration
+    /// @param feedrate maximum feedrate
-    void PlanMove(Axis axis, int16_t delta, uint16_t feedrate);
+    void PlanMove(Axis axis, pos_t delta, steps_t feedrate) {
+        PlanMoveTo(axis, CurrentPos(axis) + delta, feedrate);
+    }
 
     /// @returns current position of an axis
     /// @param axis axis affected
-    uint16_t CurrentPos(Axis axis) const;
+    pos_t CurrentPos(Axis axis) const;
+
+    /// Set acceleration for the selected axis
+    /// @param axis axis affected
+    /// @param accel acceleration
+    void SetAcceleration(Axis axis, steps_t accel) {
+        axisData[axis].ctrl.SetAcceleration(accel);
+    }
 
     /// Enqueue performing of homing of an axis
-    /// @@TODO
     void Home(Axis axis, bool direction);
 
     /// Set mode of TMC/motors operation
-    /// Common for all axes/motors
+    void SetMode(Axis axis, MotorMode mode);
-    void SetMode(hal::tmc2130::MotorMode mode);
 
     /// State machine doing all the planning and stepping preparation based on received commands
     void Step();
@@ -132,9 +126,30 @@ public:
     /// stop whatever moves are being done
     void AbortPlannedMoves();
 
-    /// probably higher-level operations knowing the semantic meaning of axes
-
 private:
+    struct AxisData {
+        TMC2130 drv; ///< Motor driver
+        pulse_gen::PulseGen ctrl; ///< Motor controller
+    };
+
+    /// Dynamic axis data
+    AxisData axisData[NUM_AXIS] = {
+        // Idler
+        {
+            .drv = { axisParams[Idler].params, axisParams[Idler].currents, axisParams[Idler].mode },
+            .ctrl = { axisParams[Idler].jerk, axisParams[Idler].accel },
+        },
+        // Pulley
+        {
+            .drv = { axisParams[Pulley].params, axisParams[Pulley].currents, axisParams[Pulley].mode },
+            .ctrl = { axisParams[Pulley].jerk, axisParams[Pulley].accel },
+        },
+        // Selector
+        {
+            .drv = { axisParams[Selector].params, axisParams[Selector].currents, axisParams[Selector].mode },
+            .ctrl = { axisParams[Selector].jerk, axisParams[Selector].accel },
+        }
+    };
 };
 
 /// ISR stepping routine

src/modules/pulse_gen.h

@@ -1,5 +1,4 @@
 #pragma once
-#include <stdint.h>
 #include "speed_table.h"
 #include "../hal/tmc2130.h"
 #include "../hal/circular_buffer.h"

tests/unit/logic/cut_filament/CMakeLists.txt

@@ -15,11 +15,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_cut_filament.cpp

tests/unit/logic/eject_filament/CMakeLists.txt

@@ -15,11 +15,14 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_gpio.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_eject_filament.cpp

tests/unit/logic/feed_to_bondtech/CMakeLists.txt

@@ -12,11 +12,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_feed_to_bondtech.cpp

tests/unit/logic/feed_to_finda/CMakeLists.txt

@@ -12,11 +12,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_feed_to_finda.cpp

tests/unit/logic/load_filament/CMakeLists.txt

@@ -14,11 +14,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_load_filament.cpp

tests/unit/logic/stubs/stub_motion.cpp

@@ -30,18 +30,11 @@ void Motion::ClearStallGuardFlag(Axis axis) {
     axes[axis].stallGuard = false;
 }
 
-void Motion::PlanMove(int16_t pulley, int16_t idler, int16_t selector, uint16_t feedrate, uint16_t starting_speed, uint16_t ending_speed) {
+void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feedrate) {
-    axes[Pulley].targetPos = axes[Pulley].pos + pulley;
+    axes[axis].targetPos = pos;
-    axes[Idler].targetPos = axes[Idler].pos + idler;
-    axes[Selector].targetPos = axes[Selector].pos + selector;
-    // speeds and feedrates are not simulated yet
 }
 
-void Motion::PlanMove(Axis axis, int16_t delta, uint16_t feedrate) {
+pos_t Motion::CurrentPos(Axis axis) const {
-    axes[axis].targetPos = axes[axis].pos + delta;
-}
-
-uint16_t Motion::CurrentPos(Axis axis) const {
     return axes[axis].pos;
 }
 
@@ -49,7 +42,7 @@ void Motion::Home(Axis axis, bool direction) {
     axes[Pulley].homed = true;
 }
 
-void Motion::SetMode(hal::tmc2130::MotorMode mode) {
+void Motion::SetMode(Axis axis, hal::tmc2130::MotorMode mode) {
 }
 
 void Motion::Step() {

tests/unit/logic/stubs/stub_motion.h

@@ -5,8 +5,8 @@ namespace modules {
 namespace motion {
 
 struct AxisSim {
-    int32_t pos;
+    pos_t pos;
-    int32_t targetPos;
+    pos_t targetPos;
     bool enabled;
     bool homed;
     bool stallGuard;

tests/unit/logic/tool_change/CMakeLists.txt

@@ -17,11 +17,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_tool_change.cpp

tests/unit/logic/unload_filament/CMakeLists.txt

@@ -14,11 +14,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_unload_filament.cpp

tests/unit/logic/unload_to_finda/CMakeLists.txt

@@ -12,11 +12,13 @@ add_executable(
   ../../../../src/modules/permanent_storage.cpp
   ../../../../src/modules/selector.cpp
   ../../../../src/modules/user_input.cpp
+  ../../../../src/modules/pulse_gen.cpp
   ../../modules/stubs/stub_adc.cpp
   ../../modules/stubs/stub_eeprom.cpp
   ../../modules/stubs/stub_gpio.cpp
   ../../modules/stubs/stub_shr16.cpp
   ../../modules/stubs/stub_timebase.cpp
+  ../../modules/stubs/stub_tmc2130.cpp
   ../stubs/main_loop_stub.cpp
   ../stubs/stub_motion.cpp
   test_unload_to_finda.cpp

tests/unit/modules/pulse_gen/test_pulse_gen.cpp

@@ -8,6 +8,7 @@ using namespace modules::pulse_gen;
 using hal::tmc2130::MotorParams;
 
 TEST_CASE("pulse_gen::basic", "[pulse_gen]") {
+    // TODO: woefully incomplete
     MotorParams mp = {
         .idx = 0,
         .dirOn = config::idler.dirOn,

tests/unit/modules/stubs/stub_tmc2130.cpp

@@ -0,0 +1,12 @@
+#include "tmc2130.h"
+
+namespace hal {
+namespace tmc2130 {
+
+TMC2130::TMC2130(const MotorParams &params,
+    const MotorCurrents &currents,
+    MotorMode mode) {
+}
+
+} // namespace tmc2130
+} // namespace hal