From 94d6795252aa970148a6a669487426a1873eefbc Mon Sep 17 00:00:00 2001
From: "D.R.racer" <drracer@drracer.eu>
Date: Fri, 17 Jun 2022 13:41:49 +0200
Subject: [PATCH] Add register for FSensorToNozzleFeedrate

---
 src/logic/feed_to_bondtech.cpp |  4 +++-
 src/modules/globals.cpp        | 11 +++++++++++
 src/modules/globals.h          | 10 ++++++++++
 src/registers.cpp              | 19 +++++++++++++++++++
 4 files changed, 43 insertions(+), 1 deletion(-)

diff --git a/src/logic/feed_to_bondtech.cpp b/src/logic/feed_to_bondtech.cpp
index 8d6d1eb..ac5eb19 100644
--- a/src/logic/feed_to_bondtech.cpp
+++ b/src/logic/feed_to_bondtech.cpp
@@ -24,7 +24,9 @@ void logic::FeedToBondtech::GoToPushToNozzle() {
     mg::globals.SetFilamentLoaded(mg::globals.ActiveSlot(), mg::FilamentLoadState::InFSensor);
     // plan a slow move to help push filament into the nozzle
     //@@TODO the speed in mm/s must correspond to printer's feeding speed!
-    mpu::pulley.PlanMove(config::fsensorToNozzle, config::pulleySlowFeedrate);
+    mpu::pulley.PlanMove(
+        config::U_mm({ (long double)mg::globals.FSensorToNozzleMM() }),
+        config::U_mm_s({ (long double)mg::globals.FSensorToNozzleFeedrate() }));
     state = PushingFilamentIntoNozzle;
 }
 
diff --git a/src/modules/globals.cpp b/src/modules/globals.cpp
index 69d7e02..7cb0eb9 100644
--- a/src/modules/globals.cpp
+++ b/src/modules/globals.cpp
@@ -20,6 +20,9 @@ void Globals::Init() {
         // the filament is not present in the selector - we can move the selector freely
         filamentLoaded = FilamentLoadState::AtPulley;
     }
+
+    ResetFSensorToNozzleMM();
+    ResetFSensorToNozzleFeedrate();
 }
 
 uint8_t Globals::ActiveSlot() const {
@@ -65,5 +68,13 @@ void Globals::SetMotorsMode(bool stealth) {
     // @@TODO store into EEPROM
 }
 
+void Globals::ResetFSensorToNozzleMM() {
+    fsensorToNozzleMM = config::fsensorToNozzle.v;
+}
+
+void Globals::ResetFSensorToNozzleFeedrate() {
+    fsensorToNozzleFeedrate = config::pulleySlowFeedrate.v;
+}
+
 } // namespace globals
 } // namespace modules
diff --git a/src/modules/globals.h b/src/modules/globals.h
index 24d9724..7fbfb4a 100644
--- a/src/modules/globals.h
+++ b/src/modules/globals.h
@@ -61,6 +61,14 @@ public:
     /// @returns true if the motors are to be operated in stealth mode
     bool MotorsStealth() const { return stealthMode; }
 
+    uint8_t FSensorToNozzleMM() const { return fsensorToNozzleMM; }
+    void ResetFSensorToNozzleMM();
+    void SetFSensorToNozzleMM(uint8_t fss2NozzleMM) { fsensorToNozzleMM = fss2NozzleMM; }
+
+    uint8_t FSensorToNozzleFeedrate() const { return fsensorToNozzleFeedrate; }
+    void ResetFSensorToNozzleFeedrate();
+    void SetFSensorToNozzleFeedrate(uint8_t fs2NozzleFeedrate) { fsensorToNozzleFeedrate = fs2NozzleFeedrate; }
+
 private:
     /// Sets the active slot, usually after some command/operation.
     /// Also updates the EEPROM records accordingly
@@ -70,6 +78,8 @@ private:
     uint8_t activeSlot;
     FilamentLoadState filamentLoaded;
     bool stealthMode;
+    uint8_t fsensorToNozzleMM;
+    uint8_t fsensorToNozzleFeedrate;
 };
 
 /// The one and only instance of global state variables
diff --git a/src/registers.cpp b/src/registers.cpp
index 12a08ed..74145a0 100644
--- a/src/registers.cpp
+++ b/src/registers.cpp
@@ -1,4 +1,8 @@
+#ifndef UNITTEST
 #include <avr/pgmspace.h>
+#else
+#define PROGMEM /* */
+#endif
 
 #include "registers.h"
 #include "version.h"
@@ -95,6 +99,21 @@ static const RegisterRec registers[] PROGMEM = {
         1),
 
     RegisterRec([]() -> uint16_t { return mg::globals.MotorsStealth(); }, 1), // mode (stealth = 1/normal = 0)
+
+    RegisterRec( // extra load distance after fsensor triggered (30mm default)
+        []() -> uint16_t { return mg::globals.FSensorToNozzleMM(); },
+        [](uint16_t d) { mg::globals.SetFSensorToNozzleMM(d); },
+        1),
+
+    // The lambas seem to be pretty cheap:
+    //    void SetFSensorToNozzleFeedrate(uint8_t fs2NozzleFeedrate) { fsensorToNozzleFeedrate = fs2NozzleFeedrate; }
+    // compiles to:
+    // sts <modules::globals::globals+0x4>, r24
+    // ret
+    RegisterRec( // extra load distance after fsensor triggered - feedrate (20mm/s default)
+        []() -> uint16_t { return mg::globals.FSensorToNozzleFeedrate(); },
+        [](uint16_t d) { mg::globals.SetFSensorToNozzleFeedrate(d); },
+        1),
 };
 
 static constexpr uint8_t registersSize = sizeof(registers) / sizeof(RegisterRec);