From e6cd5356c951215bf212d2d298b97dc8a8a81f27 Mon Sep 17 00:00:00 2001
From: Hakan Bastedt <hakan.bastedt@gmail.com>
Date: Thu, 4 Jan 2024 11:39:01 +0100
Subject: [PATCH] tests with linuxcnc, DC sync is not the way to go, leaving
 that

---
 Firmware/src/main.cpp               | 23 ++++++++++++++---------
 linuxcnc/lcec-64bitfloatadded.patch |  8 ++++----
 2 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 71f4993..493d76a 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -108,7 +108,7 @@ HardwareTimer *MyTim;
 volatile uint32_t stepCount = 0, stepPulses = 0;
 volatile double_t actualPosition = 0;
 volatile double_t requestedPosition;
-volatile uint32_t pulsesToGo = 0;
+volatile int32_t pulsesToGo = 0;
 volatile byte forwardDirection = 0; // 1 if going forward
 
 void TimerStep_CB(void)
@@ -122,7 +122,7 @@ void TimerStep_CB(void)
 
 void makePulses(uint32_t period /* in usecs */, uint32_t pulses /* nr of pulses to do*/)
 {
-   MyTim->setOverflow(pulses*1000000/period, HERTZ_FORMAT);
+   MyTim->setOverflow(pulses * 1000000 / period, HERTZ_FORMAT);
    MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 %
    stepCount = 0;
    stepPulses = pulses;
@@ -189,22 +189,27 @@ void indexPulse(void)
    }
 }
 
+uint64_t now = 0, back_then = 0;
+uint32_t deltaT;
 void sync0Handler(void)
 {
-   const int32_t stepsPerMM = 1;
+   now = micros();
+   const int32_t stepsPerMM = 1000;
    // Update the actual position
-   pulsesToGo = stepsPerMM * Obj.StepGenIn1.CommandedPosition; // Wrong, but test
+   pulsesToGo = stepsPerMM * (Obj.StepGenIn1.CommandedPosition - actualPosition);
    Obj.StepGenOut1.ActualPosition = actualPosition;
    // Get new end position
    forwardDirection = pulsesToGo > 1 ? 1 : 0;
    // Set direction pin
-   Obj.DiffT = forwardDirection;
-   digitalWrite(STEPPER_DIR_PIN, forwardDirection); // I think one should really wait a bit when changed
-
-   // Make the pulses using hardware timer
+   deltaT = now - back_then;
+   Obj.DiffT = deltaT;
    if (pulsesToGo != 0)
-      makePulses(1000 /*sync0CycleTime / 1000*/, abs((int)pulsesToGo));
+   {
+      digitalWrite(STEPPER_DIR_PIN, forwardDirection); // I think one should really wait a bit when changed
+      makePulses(deltaT-20, abs((int)pulsesToGo));        // Make the pulses using hardware timer
+   }
    actualPosition = Obj.StepGenIn1.CommandedPosition;
+   back_then = now;
 }
 
 void ESC_interrupt_enable(uint32_t mask)
diff --git a/linuxcnc/lcec-64bitfloatadded.patch b/linuxcnc/lcec-64bitfloatadded.patch
index a5ce83a..801ae31 100644
--- a/linuxcnc/lcec-64bitfloatadded.patch
+++ b/linuxcnc/lcec-64bitfloatadded.patch
@@ -39,7 +39,7 @@ index 5d9943c..261c965 100644
    lcecPdoEntTypeFloatIeee
  } LCEC_PDOENT_TYPE_T;
 diff --git a/src/lcec_generic.c b/src/lcec_generic.c
-index dfddf73..2f58fde 100644
+index dfddf73..41a13a9 100644
 --- a/src/lcec_generic.c
 +++ b/src/lcec_generic.c
 @@ -26,6 +26,7 @@ hal_s32_t lcec_generic_read_s32(uint8_t *pd, lcec_generic_pin_t *hal_data);
@@ -86,17 +86,17 @@ index dfddf73..2f58fde 100644
 +
 +  if (hal_data->pdo_bp == 0 && hal_data->bitOffset == 0) {
 +    EC_WRITE_LREAL(&pd[hal_data->pdo_os], fval);
++    return;
 +  }
 +  union {
 +    double d;
-+    uin64_t u;
++    uint64_t u;
 +  } v;
-+  v.d = fval;
++  v.d = fval; // Make an equivalent long int for bit operations (don't work on doubles)
 +  offset = ((hal_data->pdo_os << 3) | (hal_data->pdo_bp & 0x07)) + hal_data->bitOffset;
 +  for (i=0; i < hal_data->bitLength; i++, offset++) {
 +    EC_WRITE_BIT(&pd[offset >> 3], offset & 0x07,v.u & 1);
 +    v.u >>= 1;
 +  }
-+#endif
 +}
 +