Homing initial

src/config/axis.h

@@ -29,6 +29,7 @@ struct AxisConfig {
     uint8_t iHold; ///< holding current
     bool stealth; ///< Default to Stealth mode
     long double stepsPerUnit; ///< steps per unit
+    int8_t sg_thrs; /// @todo 7bit two's complement for the sg_thrs
 };
 
 /// List of available axes

src/config/config.h

@@ -99,6 +99,7 @@ static constexpr AxisConfig pulley = {
     .iHold = 0, /// 17mA in SpreadCycle, freewheel in StealthChop
     .stealth = false,
     .stepsPerUnit = (200 * 2 / 19.147274),
+    .sg_thrs = 3,
 };
 
 /// Pulley motion limits
@@ -120,6 +121,7 @@ static constexpr AxisConfig selector = {
     .iHold = 5, /// 99mA
     .stealth = false,
     .stepsPerUnit = (200 * 8 / 8.),
+    .sg_thrs = 3,
 };
 
 /// Selector motion limits
@@ -168,6 +170,7 @@ static constexpr AxisConfig idler = {
     .iHold = 23, /// 398mA
     .stealth = false,
     .stepsPerUnit = (200 * 16 / 360.),
+    .sg_thrs = 8,
 };
 
 /// Idler motion limits
@@ -198,10 +201,10 @@ static constexpr U_deg_s idlerFeedrate = 200._deg_s;
 
 // TMC2130 setup
 
-static constexpr int8_t tmc2130_sg_thrs = 3; // @todo 7bit two's complement for the sg_thrs
-static_assert(tmc2130_sg_thrs >= -64 && tmc2130_sg_thrs <= 63, "tmc2130_sg_thrs out of range");
+// static constexpr int8_t tmc2130_sg_thrs = 3;
+// static_assert(tmc2130_sg_thrs >= -64 && tmc2130_sg_thrs <= 63, "tmc2130_sg_thrs out of range");
 
-static constexpr uint32_t tmc2130_coolStepThreshold = 400; ///< step-based 20bit uint
+static constexpr uint32_t tmc2130_coolStepThreshold = 5000; ///< step-based 20bit uint
 static_assert(tmc2130_coolStepThreshold <= 0xfffff, "tmc2130_coolStepThreshold out of range");
 
 static constexpr uint32_t tmc2130_PWM_AMPL = 240;

src/hal/tmc2130.cpp

@@ -2,10 +2,14 @@
 #include "tmc2130.h"
 #include "../config/config.h"
 
+#include "debug.h"
+
 namespace hal {
 namespace tmc2130 {
 
 bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, MotorMode mode) {
+    sg_filter_threshold = (1 << (8 - params.mRes)) - 1;
+
     gpio::Init(params.csPin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::high));
     gpio::Init(params.sgPin, gpio::GPIO_InitTypeDef(gpio::Mode::input, gpio::Pull::up));
     gpio::Init(params.stepPin, gpio::GPIO_InitTypeDef(gpio::Mode::output, gpio::Level::low));
@@ -39,7 +43,7 @@ bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, Mot
     WriteRegister(params, Registers::TPOWERDOWN, 0);
 
     ///Stallguard parameters
-    static constexpr uint32_t tmc2130_coolConf = (((uint32_t)config::tmc2130_sg_thrs) << 16U);
+    uint32_t tmc2130_coolConf = (((uint32_t)params.sg_thrs) << 16U);
     WriteRegister(params, Registers::COOLCONF, tmc2130_coolConf);
     WriteRegister(params, Registers::TCOOLTHRS, config::tmc2130_coolStepThreshold);
 
@@ -78,15 +82,10 @@ void TMC2130::SetCurrents(const MotorParams &params, const MotorCurrents &curren
 void TMC2130::SetEnabled(const MotorParams &params, bool enabled) {
     hal::shr16::shr16.SetTMCEnabled(params.idx, enabled);
     if (this->enabled != enabled)
-        ClearStallguard(params);
+        ClearStallguard();
     this->enabled = enabled;
 }
 
-void TMC2130::ClearStallguard(const MotorParams &params) {
-    // @@TODO maximum resolution right now is x256/4 (uint8_t / 4)
-    sg_counter = 4 * (1 << (8 - params.mRes)) - 1; /// one electrical full step (4 steps when fullstepping)
-}
-
 bool TMC2130::CheckForErrors(const MotorParams &params) {
     uint32_t GSTAT = ReadRegister(params, Registers::GSTAT);
     uint32_t DRV_STATUS = ReadRegister(params, Registers::DRV_STATUS);
@@ -115,11 +114,11 @@ void TMC2130::WriteRegister(const MotorParams &params, Registers reg, uint32_t d
 }
 
 void TMC2130::Isr(const MotorParams &params) {
-    if (sg_counter) {
+    if (sg_filter_counter) {
         if (SampleDiag(params))
-            sg_counter--;
-        else if (sg_counter < (4 * (1 << (8 - params.mRes)) - 1))
-            sg_counter++;
+            sg_filter_counter--;
+        else if (sg_filter_counter < sg_filter_threshold)
+            sg_filter_counter++;
     }
 }
 

src/hal/tmc2130.h

@@ -25,6 +25,7 @@ struct MotorParams {
     gpio::GPIO_pin stepPin; ///< step pin
     gpio::GPIO_pin sgPin; ///< stallguard pin
     config::MRes mRes; ///< microstep resolution
+    int8_t sg_thrs;
 };
 
 struct MotorCurrents {
@@ -114,10 +115,12 @@ public:
     }
 
     inline bool Stalled() const {
-        return sg_counter == 0;
+        return sg_filter_counter == 0;
     }
 
-    void ClearStallguard(const MotorParams &params);
+    inline void ClearStallguard() {
+        sg_filter_counter = sg_filter_threshold;
+    }
 
     /// Should be called periodically from main loop. Maybe not all the time. Once every 10 ms is probably enough
     bool CheckForErrors(const MotorParams &params);
@@ -149,7 +152,8 @@ private:
 
     ErrorFlags errorFlags;
     bool enabled = false;
-    uint8_t sg_counter = 0;
+    uint8_t sg_filter_threshold;
+    uint8_t sg_filter_counter = 0;
 };
 
 } // namespace tmc2130

src/main.cpp

@@ -145,6 +145,9 @@ void setup() {
 
     mu::cdc.Init();
 
+    ms::selector.Home();
+    mi::idler.Home();
+
     _delay_ms(100);
 
     /// Turn off all leds

src/modules/idler.cpp

@@ -21,6 +21,10 @@ void Idler::PlanHomingMove() {
     dbg_logic_P(PSTR("Plan Homing Idler"));
 }
 
+void Idler::FinishHoming() {
+    mm::motion.SetPosition(mm::Idler, 0);
+}
+
 Idler::OperationResult Idler::Disengage() {
     if (state == Moving) {
         dbg_logic_P(PSTR("Moving --> Disengage refused"));

src/modules/idler.h

@@ -52,6 +52,7 @@ public:
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
     virtual void PlanHomingMove() override;
+    virtual void FinishHoming() override;
 
 private:
     /// direction of travel - engage/disengage

src/modules/motion.cpp

@@ -43,7 +43,7 @@ bool Motion::StallGuard(Axis axis) {
 }
 
 void Motion::StallGuardReset(Axis axis) {
-    axisData[axis].drv.ClearStallguard(axisParams[axis].params);
+    axisData[axis].drv.ClearStallguard();
 }
 
 void Motion::PlanMoveTo(Axis axis, pos_t pos, steps_t feed_rate, steps_t end_rate) {

src/modules/motion.h

@@ -42,7 +42,7 @@ static AxisParams axisParams[NUM_AXIS] = {
     // Pulley
     {
         .name = 'P',
-        .params = { .spi = hal::spi::TmcSpiBus, .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .mRes = config::pulley.mRes },
+        .params = { .spi = hal::spi::TmcSpiBus, .idx = Pulley, .dirOn = config::pulley.dirOn, .csPin = PULLEY_CS_PIN, .stepPin = PULLEY_STEP_PIN, .sgPin = PULLEY_SG_PIN, .mRes = config::pulley.mRes, .sg_thrs = config::pulley.sg_thrs },
         .currents = { .vSense = config::pulley.vSense, .iRun = config::pulley.iRun, .iHold = config::pulley.iHold },
         .mode = DefaultMotorMode(config::pulley),
         .jerk = unitToSteps<P_speed_t>(config::pulleyLimits.jerk),
@@ -51,7 +51,7 @@ static AxisParams axisParams[NUM_AXIS] = {
     // Selector
     {
         .name = 'S',
-        .params = { .spi = hal::spi::TmcSpiBus, .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .mRes = config::selector.mRes },
+        .params = { .spi = hal::spi::TmcSpiBus, .idx = Selector, .dirOn = config::selector.dirOn, .csPin = SELECTOR_CS_PIN, .stepPin = SELECTOR_STEP_PIN, .sgPin = SELECTOR_SG_PIN, .mRes = config::selector.mRes, .sg_thrs = config::selector.sg_thrs },
         .currents = { .vSense = config::selector.vSense, .iRun = config::selector.iRun, .iHold = config::selector.iHold },
         .mode = DefaultMotorMode(config::selector),
         .jerk = unitToSteps<S_speed_t>(config::selectorLimits.jerk),
@@ -60,7 +60,7 @@ static AxisParams axisParams[NUM_AXIS] = {
     // Idler
     {
         .name = 'I',
-        .params = { .spi = hal::spi::TmcSpiBus, .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .mRes = config::idler.mRes },
+        .params = { .spi = hal::spi::TmcSpiBus, .idx = Idler, .dirOn = config::idler.dirOn, .csPin = IDLER_CS_PIN, .stepPin = IDLER_STEP_PIN, .sgPin = IDLER_SG_PIN, .mRes = config::idler.mRes, .sg_thrs = config::idler.sg_thrs },
         .currents = { .vSense = config::idler.vSense, .iRun = config::idler.iRun, .iHold = config::idler.iHold },
         .mode = DefaultMotorMode(config::idler),
         .jerk = unitToSteps<I_speed_t>(config::idlerLimits.jerk),

src/modules/movable_base.cpp

@@ -8,13 +8,13 @@ namespace motion {
 
 void MovableBase::PlanHome(config::Axis axis) {
     // switch to normal mode on this axis
-    state = Homing;
     mm::motion.InitAxis(axis);
     mm::motion.SetMode(axis, mm::Normal);
     mm::motion.StallGuardReset(axis);
 
     // plan move at least as long as the axis can go from one side to the other
     PlanHomingMove(); // mm::motion.PlanMove(axis, delta, 1000);
+    state = Homing;
 }
 
 MovableBase::OperationResult MovableBase::InitMovement(config::Axis axis) {
@@ -44,6 +44,7 @@ void MovableBase::PerformHome(config::Axis axis) {
         // we have reached the end of the axis - homed ok
         mm::motion.AbortPlannedMoves(axis, true);
         mm::motion.SetMode(axis, mg::globals.MotorsStealth() ? mm::Stealth : mm::Normal);
+        FinishHoming();
         state = Ready;
     } else if (mm::motion.QueueEmpty(axis)) {
         // we ran out of planned moves but no StallGuard event has occurred - homing failed

src/modules/movable_base.h

@@ -27,8 +27,8 @@ public:
 
     inline constexpr MovableBase()
         : state(Ready)
-        , plannedSlot(0)
-        , currentSlot(0) {}
+        , plannedSlot(-1)
+        , currentSlot(-1) {}
 
     /// virtual ~MovableBase(); intentionally disabled, see description in logic::CommandBase
 
@@ -60,6 +60,7 @@ protected:
 
     virtual void PrepareMoveToPlannedSlot() = 0;
     virtual void PlanHomingMove() = 0;
+    virtual void FinishHoming() = 0;
 
     OperationResult InitMovement(config::Axis axis);
 

src/modules/selector.cpp

@@ -21,6 +21,10 @@ void Selector::PlanHomingMove() {
     dbg_logic_P(PSTR("Plan Homing Selector"));
 }
 
+void Selector::FinishHoming() {
+    mm::motion.SetPosition(mm::Selector, mm::unitToSteps<mm::S_pos_t>(config::selectorLimits.lenght));
+}
+
 Selector::OperationResult Selector::MoveToSlot(uint8_t slot) {
     if (state == Moving) {
         dbg_logic_P(PSTR("Moving --> Selector refused"));

src/modules/selector.h

@@ -41,6 +41,7 @@ public:
 protected:
     virtual void PrepareMoveToPlannedSlot() override;
     virtual void PlanHomingMove() override;
+    virtual void FinishHoming() override;
 
 private:
 };