From 544dd5ed8563e20d7f48bf3d187c55e634aa5fb6 Mon Sep 17 00:00:00 2001
From: Hakan Bastedt <hakan.bastedt@gmail.com>
Date: Fri, 5 Apr 2024 18:30:33 +0200
Subject: [PATCH] I works. Use 4.19-rt kernel Ethernet Intel I210 controller
 ethtool -C enp1s0 rx-usecs 0 tx-usecs 0 2 ms loop time in linuxcnc .ini and
 .xml and code baseTimer period can be adjusted startTime for baseTimer can
 also be adjusted.

---
 Firmware/include/StepGen3.h |  2 +-
 Firmware/src/main.cpp       | 23 +++++++++++++----------
 2 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/Firmware/include/StepGen3.h b/Firmware/include/StepGen3.h
index 82a1939..7247dcb 100755
--- a/Firmware/include/StepGen3.h
+++ b/Firmware/include/StepGen3.h
@@ -147,7 +147,7 @@ private:
 
 // For the example
 #define BASE_PERIOD 40000 
-#define SERVO_PERIOD 1000000
+#define SERVO_PERIOD 2000000
 #define JOINT_X_STEPGEN_MAXACCEL 0.0
 #define JOINT_Z_STEPGEN_MAXACCEL 0.0
 #define JOINT_X_SCALE -200
diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp
index 3340a14..1aa63d6 100755
--- a/Firmware/src/main.cpp
+++ b/Firmware/src/main.cpp
@@ -7,7 +7,7 @@ extern "C"
 };
 _Objects Obj;
 
-#define NEEDED 0
+#define NEEDED 1
 
 HardwareSerial Serial1(PA10, PA9);
 
@@ -64,7 +64,7 @@ void syncWithLCNC()
    prevMakePulsesCnt = makePulsesCnt;
    Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions
    Step->makeAllPulses();                   // Make first step right here
-   basePeriodCnt = 1000000 / BASE_PERIOD;   //
+   basePeriodCnt = SERVO_PERIOD / BASE_PERIOD;   //
    baseTimer->refresh();                    //
    baseTimer->resume();                     // Make the other steps in ISR
    // baseTimer->isRunning();
@@ -115,9 +115,10 @@ void handleStepper(void)
    Step->stepgen_array[0].pos_scale = -Obj.StepsPerMM1;
    Step->stepgen_array[1].pos_scale = -Obj.StepsPerMM2;
 
-   Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
-   Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
+   // Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb;
+   // Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb;
    uint32_t diffT = longTime.extendTime(micros()) - irqTime;
+   maxCycleTime = 600;
    delayT = maxCycleTime + 50 - diffT; // Add 50 as some saftey margin
    if (delayT > 0 && delayT < 900)
    {
@@ -134,6 +135,7 @@ void handleStepper(void)
 uint16_t oldCnt = 0;
 uint64_t startTime = 0;
 uint16_t avgTime = 0;
+float_t oldCommandedPosition = 0;
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
 {
 // Obj.IndexStatus = Encoder1.indexHappened();
@@ -163,10 +165,11 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
 
    // Obj.DiffT = longTime.extendTime(micros()) - irqTime; // max_Tim - min_Tim; // Debug
    Obj.DiffT = nLoops;
-   Obj.D1 = 1000 * Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
-   Obj.D2 = 1000 * Obj.ActualPosition2;    // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX;                          // Step->stepgen_array[1].freq;
-   Obj.D3 = nLoops;                        // Step->stepgen_array[1].freq;
-   Obj.D4 = 0;                             // deltaMakePulsesCnt;            // Step->stepgen_array[1].rawcount % UINT16_MAX;
+   Obj.D1 = 1000 * Obj.CommandedPosition2;                          // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos();
+   Obj.D2 = 1000 * Obj.ActualPosition2;                             // Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX;                          // Step->stepgen_array[1].freq;
+   Obj.D3 = irqCnt % 256;                                           // Step->stepgen_array[1].freq;
+   Obj.D4 = 1000 * (Obj.CommandedPosition2 - oldCommandedPosition); // deltaMakePulsesCnt;            // Step->stepgen_array[1].rawcount % UINT16_MAX;
+   oldCommandedPosition = Obj.CommandedPosition2;
 }
 
 void ESC_interrupt_enable(uint32_t mask);
@@ -217,7 +220,7 @@ void setup(void)
    baseTimer = new HardwareTimer(TIM11); // The base period timer
    uint32_t usecs = BASE_PERIOD / sync0CycleTime;
    // setFrequencyAdjustedMicrosSeconds(baseTimer, usecs);
-   baseTimer->setOverflow(20, MICROSEC_FORMAT);
+   baseTimer->setOverflow(35, MICROSEC_FORMAT);
    baseTimer->attachInterrupt(basePeriodCB);
 
    syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer
@@ -248,7 +251,7 @@ void loop(void)
 void sync0Handler(void)
 {
    ALEventIRQ = ESC_ALeventread();
-   if (ALEventIRQ & ESCREG_ALEVENT_SM2)
+   // if (ALEventIRQ & ESCREG_ALEVENT_SM2)
    {
       irqTime = longTime.extendTime(micros());
       serveIRQ = 1;