From e7fc20fec59861eb4543172b4dc037e1b168751f Mon Sep 17 00:00:00 2001
From: Hakan Bastedt <hakan.bastedt@gmail.com>
Date: Tue, 23 Jan 2024 22:40:15 +0100
Subject: [PATCH] micros() in irq is suspicous. Some optomization of stepper
 parameters. Comments.

---
 Firmware/src/StepGen.cpp | 28 ++++++++++++++++++++--------
 Firmware/src/main.cpp    | 10 +++++-----
 2 files changed, 25 insertions(+), 13 deletions(-)

diff --git a/Firmware/src/StepGen.cpp b/Firmware/src/StepGen.cpp
index 9f9bcef..d4850d1 100755
--- a/Firmware/src/StepGen.cpp
+++ b/Firmware/src/StepGen.cpp
@@ -51,12 +51,25 @@ void StepGen::handleStepper(void)
 
     actPos(timerStepPosition / double(stepsPerMM));
     double diffPosition = reqPos() - actPos();
-    // Wild "tone" kludge
-    if (abs(diffPosition) > 0.0005) // 60 mm/min = 0.001 mm/ms
+#if 1
+// Wild "tone" kludge. map() function
+#define SPEED_MIN 0.00005
+#define SPEED_MAX 0.0005
+#define FACT_LOW 1.0
+#define FACT_HIGH 20.0
+    if (abs(diffPosition) < SPEED_MIN) // 60 mm/min = 0.001 mm/ms
     {
-        pwmCycleTime = 10 * StepGen::sync0CycleTime;
+        pwmCycleTime = FACT_LOW * StepGen::sync0CycleTime;
     }
-
+    else if (abs(diffPosition) > SPEED_MAX) // 60 mm/min = 0.001 mm/ms
+    {
+        pwmCycleTime = FACT_HIGH * StepGen::sync0CycleTime;
+    }
+    else
+    {
+        pwmCycleTime = (FACT_LOW + (FACT_HIGH - FACT_LOW) * (abs(diffPosition) - SPEED_MIN) / (SPEED_MAX - SPEED_MIN)) * StepGen::sync0CycleTime;
+    }
+#endif
     uint64_t fre = abs(diffPosition) * stepsPerMM * 1000000 / double(pwmCycleTime); // Frequency needed
     if (fre > maxFreq)                                                              // Only do maxFre
     {
@@ -68,9 +81,8 @@ void StepGen::handleStepper(void)
     // Will be picked up by the timer_CB and the timer is reloaded.
     timerNewEndStepPosition = pulsesAtEndOfCycle;
 
-    if (!timerIsRunning) // no timer isn't running. start it here
+    if (!timerIsRunning) // Timer isn't running. Start it here
     {
-        // Start the timer
         int32_t steps = pulsesAtEndOfCycle - timerStepPosition; // Pulses to go + or -
         if (steps != 0)
         {
@@ -93,8 +105,8 @@ void StepGen::timerCB()
     timerStepPosition += timerStepDirection; // The step that was just completed
     if (timerNewEndStepPosition != 0)        // Are we going to reload?
     {
-        // Input for reload is timerNewEndStepPosition and timerNewEndTime
-        // The timer has current position and current time and from this
+        // Input for reload is timerNewEndStepPosition
+        // The timer has current position and from this
         // can set new frequency and new endtarget for steps
         MyTim->pause(); // We are not at stop, let's stop it
         int32_t steps = timerNewEndStepPosition - timerStepPosition;
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index cc41a62..4106e83 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -66,7 +66,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    Obj.StepGenOut1.ActualPosition = Step1.actPos();
    Obj.StepGenOut2.ActualPosition = Step2.actPos();
 
-   uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 microseconds
+   uint32_t dTim = nowTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds
    Tim.push(dTim);
    uint32_t max_Tim = 0, min_Tim = UINT32_MAX;
    for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++)
@@ -119,19 +119,19 @@ void loop(void)
 {
    if (serveIRQ)
    {
+      nowTime = micros();
       DIG_process(DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG |
                   DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG);
       serveIRQ = 0;
       ESCvar.PrevTime = ESCvar.Time;
    }
-   uint32_t dTime=micros()-nowTime;
-   if ((dTime > 100 && dTime < 800) || dTime > 1500) // Don't run ecat_slv_poll when expecting to server interrupt
-      ecat_slv_poll(); 
+   uint32_t dTime = micros() - nowTime;
+   if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt
+      ecat_slv_poll();
 }
 
 void sync0Handler(void)
 {
-   nowTime = micros();
    serveIRQ = 1;
 }