From 989b6e6191dad193ca7b59447f2547f4fe778440 Mon Sep 17 00:00:00 2001
From: Alex Voinea <voinea.dragos.alexandru@gmail.com>
Date: Sun, 31 Jul 2022 20:38:37 +0300
Subject: [PATCH] homing changes to increase reliability

---
 src/config/config.h      | 4 +++-
 src/hal/tmc2130.cpp      | 3 ++-
 src/modules/idler.cpp    | 4 ++--
 src/modules/selector.cpp | 4 ++--
 4 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/src/config/config.h b/src/config/config.h
index 1b090b7..fad203f 100644
--- a/src/config/config.h
+++ b/src/config/config.h
@@ -168,6 +168,7 @@ static constexpr U_mm selectorSlotPositions[toolCount + 1] = {
 };
 
 static constexpr U_mm_s selectorFeedrate = 45._mm_s;
+static constexpr U_mm_s selectorHomingFeedrate = 30._mm_s;
 /// End: Selector configuration
 
 /// Begin: Idler configuration
@@ -179,7 +180,7 @@ static constexpr AxisConfig idler = {
     .iHold = 23, /// 398mA
     .stealth = false,
     .stepsPerUnit = (200 * 16 / 360.),
-    .sg_thrs = 7,
+    .sg_thrs = 5,
 };
 
 /// Idler motion limits
@@ -206,6 +207,7 @@ static constexpr U_deg idlerSlotPositions[toolCount + 1] = {
 static constexpr U_deg idlerParkPositionDelta = -IdlerSlotDistance + 5.0_deg / 2; ///@TODO verify
 
 static constexpr U_deg_s idlerFeedrate = 300._deg_s;
+static constexpr U_deg_s idlerHomingFeedrate = 265._deg_s;
 /// End: Idler configuration
 
 // TMC2130 setup
diff --git a/src/hal/tmc2130.cpp b/src/hal/tmc2130.cpp
index 2e3a336..89951b6 100644
--- a/src/hal/tmc2130.cpp
+++ b/src/hal/tmc2130.cpp
@@ -8,7 +8,8 @@ namespace hal {
 namespace tmc2130 {
 
 bool TMC2130::Init(const MotorParams &params, const MotorCurrents &currents, MotorMode mode) {
-    sg_filter_threshold = (1 << (8 - params.mRes));
+    // sg_filter_threshold = (1 << (8 - params.mRes));
+    sg_filter_threshold = 2;
 
     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));
diff --git a/src/modules/idler.cpp b/src/modules/idler.cpp
index 6ca0d77..21c7dfd 100644
--- a/src/modules/idler.cpp
+++ b/src/modules/idler.cpp
@@ -18,7 +18,7 @@ void Idler::PrepareMoveToPlannedSlot() {
 }
 
 void Idler::PlanHomingMoveForward() {
-    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
+    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerHomingFeedrate));
     dbg_logic_P(PSTR("Plan Homing Idler Forward"));
 }
 
@@ -26,7 +26,7 @@ void Idler::PlanHomingMoveBack() {
     // we expect that we are at the front end of the axis, set the expected axis' position
     mm::motion.SetPosition(mm::Idler, mm::unitToSteps<mm::I_pos_t>(config::idlerLimits.lenght));
     axisStart = mm::axisUnitToTruncatedUnit<config::U_deg>(mm::motion.CurPosition<mm::Idler>());
-    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(-config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerFeedrate));
+    mm::motion.PlanMove<mm::Idler>(mm::unitToAxisUnit<mm::I_pos_t>(-config::idlerLimits.lenght * 2), mm::unitToAxisUnit<mm::I_speed_t>(config::idlerHomingFeedrate));
     dbg_logic_P(PSTR("Plan Homing Idler Back"));
 }
 
diff --git a/src/modules/selector.cpp b/src/modules/selector.cpp
index 5d55ac9..cceb713 100644
--- a/src/modules/selector.cpp
+++ b/src/modules/selector.cpp
@@ -28,7 +28,7 @@ void Selector::PlanHomingMoveBack() {
     // we expect that we are at the front end of the axis, set the expected axis' position
     mm::motion.SetPosition(mm::Selector, 0);
     axisStart = mm::axisUnitToTruncatedUnit<config::U_mm>(mm::motion.CurPosition<mm::Selector>());
-    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+    mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorHomingFeedrate));
     dbg_logic_P(PSTR("Plan Homing Selector Back"));
 }
 
@@ -101,7 +101,7 @@ bool Selector::Step() {
         if (mi::idler.HomingValid()) {
             // idler is ok, we can start homing the selector
             state = HomeForward;
-            mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorFeedrate));
+            mm::motion.PlanMove<mm::Selector>(mm::unitToAxisUnit<mm::S_pos_t>(-config::selectorLimits.lenght * 2), mm::unitToAxisUnit<mm::S_speed_t>(config::selectorHomingFeedrate));
         }
         return false;
     case HomeForward: