From 2a1111c5372cfbaa4333bc5dcbfd3d397954724a Mon Sep 17 00:00:00 2001
From: Hakan Bastedt <hakan.bastedt@gmail.com>
Date: Thu, 28 Dec 2023 18:46:13 +0100
Subject: [PATCH] Updates to encoder

---
 Pcb-1-lan9252/Firmware/src/main.cpp              | 15 +++++++++------
 Pcb-1-lan9252/linuxcnc/Turner.hal                |  8 ++++----
 Pcb-1-lan9252/linuxcnc/metalmusings_encoder.comp | 10 ----------
 3 files changed, 13 insertions(+), 20 deletions(-)

diff --git a/Pcb-1-lan9252/Firmware/src/main.cpp b/Pcb-1-lan9252/Firmware/src/main.cpp
index e75953e..46bb0ae 100755
--- a/Pcb-1-lan9252/Firmware/src/main.cpp
+++ b/Pcb-1-lan9252/Firmware/src/main.cpp
@@ -28,13 +28,14 @@ uint32_t CurPosScale = 1;
 uint8_t OldLatchCEnable = 0;
 volatile uint8_t indexPulseFired = 0;
 uint32_t nFires = 0;
-volatile uint8_t pleaseZeroTheCounter=0;
+volatile uint8_t pleaseZeroTheCounter = 0;
+uint32_t PrevTime = 0, Prev2Time = 0;
 
 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
 {
    if (Obj.IndexLatchEnable && !OldLatchCEnable) // Should only happen first time IndexCEnable is set
    {
-      pleaseZeroTheCounter=1;
+      pleaseZeroTheCounter = 1;
    }
    OldLatchCEnable = Obj.IndexLatchEnable;
 
@@ -55,7 +56,10 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
       nFires++;
       PreviousEncoderCounterValue = 0;
    }
-   Obj.DiffT = nFires;
+   uint64_t now = micros();
+   Obj.DiffT = now - Prev2Time;
+   Prev2Time = PrevTime;
+   PrevTime = now;
 
    int64_t pos = unwrap_encoder(TIM2->CNT, &PreviousEncoderCounterValue);
    double CurPos = pos * PosScaleRes;
@@ -63,8 +67,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
 
    double diffT = 0;
    double diffPos = 0;
-   TDelta.push(ESCvar.Time); // Too bad resolution to measure over 1 ms. The length of the circular buffers
-   Pos.push(CurPos);         // tells over how long time the position is measured.
+   TDelta.push(ESCvar.Time); // Running average over the length of the circular buffer
+   Pos.push(CurPos);
    if (Pos.size() >= 2)
    {
       diffT = 1.0e-9 * (TDelta.last() - TDelta.first()); // Time is in nanoseconds
@@ -80,7 +84,6 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
       nFires++;
       PreviousEncoderCounterValue = 0;
    }
-   Obj.DiffT = nFires;
    Obj.IndexByte = digitalRead(INDEX_PIN);
    if (Obj.IndexByte)
       Serial1.printf("IS 1\n");
diff --git a/Pcb-1-lan9252/linuxcnc/Turner.hal b/Pcb-1-lan9252/linuxcnc/Turner.hal
index a009a81..906abe5 100644
--- a/Pcb-1-lan9252/linuxcnc/Turner.hal
+++ b/Pcb-1-lan9252/linuxcnc/Turner.hal
@@ -97,10 +97,10 @@ net z-drv-target-velo     cia402.1.drv-target-velocity => lcec.0.5.target-veloci
 setp iocontrol.0.emc-enable-in 1
 #net estop iocontrol.0.emc-enable-in lcec.0.1.din-0
 
-#setp lcec.0.3.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
+setp lcec.0.3.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
 setp lcec.0.6.enc-pos-scale [SPINDLE_9]ENCODER_SCALE
 
-#net spindle-revs <= lcec.0.3.enc-pos
+net spindle-revs <= lcec.0.3.enc-pos
 net spindle-revs <= lcec.0.6.enc-position
 loadrt invert 
 loadrt mult2    names=mult2.rps,mult2.rpm
@@ -112,10 +112,10 @@ setp invert.0.in          [SPINDLE_9]ENCODER_SCALE
 setp mult2.rpm.in0        -60.0
 setp mult2.rpm.in0        60.0
 #net  enc-invert-pos-scale mult2.rps.in0  <= invert.0.out
-#net  enc-get-freq-rps     mult2.rps.in1  <= lcec.0.3.enc-frequency
+net  enc-get-freq-rps     mult2.rps.in1  <= lcec.0.3.enc-frequency
 #net  spindle-vel-fb-rps   mult2.rpm.in1  <= mult2.rps.out
 #net  spindle-vel-fb-rpm   mult2.rpm.out
-#net  spindle-index-enable lcec.0.3.enc-index-c-enable
+net  spindle-index-enable lcec.0.3.enc-index-c-enable
 
 net  spindle-vel-fb-rps   mult2.rpm.in1  <= lcec.0.6.enc-frequency
 net  spindle-vel-fb-rpm   mult2.rpm.out
diff --git a/Pcb-1-lan9252/linuxcnc/metalmusings_encoder.comp b/Pcb-1-lan9252/linuxcnc/metalmusings_encoder.comp
index 19b9142..80c146e 100644
--- a/Pcb-1-lan9252/linuxcnc/metalmusings_encoder.comp
+++ b/Pcb-1-lan9252/linuxcnc/metalmusings_encoder.comp
@@ -7,16 +7,6 @@ function _;
 license "GPL";
 ;;
 
-// general pin set true function
-void set(hal_bit_t *p){
-    *p = 1;
-}
-
-// general pin set false function
-void unset(hal_bit_t *p){
-    *p = 0;
-}
-
 //main function
 FUNCTION(_) {
     index_latch_enable = index_c_enable;