Going for test in the lathe

Firmware/include/StepGen2.h

@@ -5,23 +5,23 @@
 
 class StepGen2
 {
-private:
+public:
     volatile double_t actualPosition;
     volatile int32_t nSteps;
     volatile uint32_t timerPulseSteps;
+    volatile uint32_t timerFrequency;
 
 public:
-    volatile float Tstartf;    // Starting delay in secs
+    volatile float Tstartf;                   // Starting delay in secs
-    volatile uint32_t Tstartu; // Starting delay in usecs
+    volatile uint32_t Tstartu;                // Starting delay in usecs
-private:
+    volatile float Tpulses;                   // Time it takes to do pulses. Debug
-public:
     const float maxAllowedFrequency = 100000; // 100 kHz for now
     HardwareTimer *pulseTimer;
     uint32_t pulseTimerChan;
     HardwareTimer *startTimer; // 10,11,13,14
     uint8_t dirPin;
     PinName stepPin;
-    const float Tjitter = 500.0; // Time unit is microseconds
+    const uint32_t Tjitter = 500; // Time unit is microseconds
     uint64_t dbg;
 
 public:
@@ -38,10 +38,19 @@ public:
 
     StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void));
 
-    uint32_t handleStepper(uint64_t irqTime/* time for irq nanosecs */);
+    uint32_t handleStepper(uint64_t irqTime /* time for irq nanosecs */);
     void startTimerCB();
     void pulseTimerCB();
     uint32_t updatePos(uint32_t i);
 };
 
+class extend32to64
+{
+public:
+    int64_t previousTimeValue = 0;
+    const uint64_t ONE_PERIOD = 4294967296; // almost UINT32_MAX;
+    const uint64_t HALF_PERIOD = 2147483648;
+    int64_t extendTime(uint32_t in);
+};
+
 #endif

Firmware/src/StepGen2.cpp

@@ -5,7 +5,7 @@ extern "C"
 {
 #include "esc.h"
 }
-extern int64_t extendTime(uint32_t);
+extern extend32to64 longTime;
 
 StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void))
 {
@@ -21,7 +21,7 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
     stepPin = _stepPin;
     pulseTimerChan = _timerChannel;
     pulseTimer = new HardwareTimer(Timer);
-    pulseTimer->attachInterrupt(irq);
+    pulseTimer->attachInterrupt(pulseTimerChan, irq); // Capture/compare innterrupt
     pinMode(dirPin, OUTPUT);
     startTimer = new HardwareTimer(Timer2);
     startTimer->attachInterrupt(irq2);
@@ -48,10 +48,12 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime)
                                                                          // Operating on incoming positions (not steps)
                                                                          //   if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01)                 // Very flat slope
     nSteps = commandedStepPosition - initialStepPosition;                //
-    if (abs(nSteps) <= 8)                                                // Some small number
+
-    {                                                                    //
+    if (abs(nSteps) < 1000) // Some small number
-        Tstartf = 0;                                                     // Just take a step in the middle of the cycle
+    {                      //
-        frequency = 1000 * (abs(nSteps) + 1);                            // At some suitable frequency
+        frequency = (abs(nSteps) + 1) / lcncCycleTime;
+        Tpulses = abs(nSteps) / frequency;
+        Tstartf = (lcncCycleTime - Tpulses) / 2.0;
     }
     else // Regular step train, up or down
     {
@@ -59,15 +61,15 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime)
             Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ;
         else
             Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ;
-        frequency = fabs(kTRAJ * stepsPerMM);
+        frequency = fabs(kTRAJ * stepsPerMM); //
-        nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction.
+        Tpulses = abs(nSteps) / frequency;
     }
     updatePos(5);
-    uint64_t nowTime = extendTime(micros()); // usecs
+    uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
-    dbg = nowTime - irqTime;
+    dbg = timeSinceISR;
-    Tstartu = Tjitter + Tstartf * 1e6 // Was secs, now usecs
+    Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
-              - (nowTime - irqTime);  // Have already wasted some time since the irq.
 
+    timerFrequency = uint32_t(frequency);
     startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
     startTimer->resume();
     return 1;
@@ -76,11 +78,10 @@ void StepGen2::startTimerCB()
 {
     digitalWrite(dirPin, cnt++ % 2);
     startTimer->pause(); // Once is enough.
-    // digitalWrite(dirPin, nSteps > 0 ? 1 : -1);
     timerPulseSteps = abs(nSteps);
     pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
-    pulseTimer->setOverflow(uint32_t(frequency), HERTZ_FORMAT);
+    pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
-    pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT); // 50%
+    pulseTimer->setCaptureCompare(pulseTimerChan, 5, MICROSEC_COMPARE_FORMAT); // 5 usecs
     pulseTimer->resume();
 }
 void StepGen2::pulseTimerCB()
@@ -101,3 +102,20 @@ uint32_t StepGen2::updatePos(uint32_t i)
 }
 
 uint32_t StepGen2::sync0CycleTime = 0;
+
+// Extend from 32-bit to 64-bit precision
+int64_t extend32to64::extendTime(uint32_t in)
+{
+    int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
+    int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current
+
+    // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
+    int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
+    if (dif < -HALF_PERIOD)
+        mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
+
+    int64_t unwrapped = previousTimeValue + mod_dif;
+    previousTimeValue = unwrapped; // load previous value
+
+    return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
+}

Firmware/src/main.cpp

@@ -27,7 +27,7 @@ void startTimerCallback(void) { Step.startTimerCB(); }
 CircularBuffer<uint32_t, 200> Tim;
 volatile uint64_t irqTime = 0, thenTime = 0;
 volatile uint32_t ccnnt = 0;
-int64_t extendTime(uint32_t in); // Extend from 32-bit to 64-bit precision
+extend32to64 longTime;
 
 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation
 {
@@ -42,7 +42,7 @@ void handleStepper(void)
    Step.commandedPosition = Obj.StepGenIn1.CommandedPosition;
    Obj.StepGenOut1.ActualPosition = Step.commandedPosition;
    Step.stepsPerMM = Obj.StepGenIn1.StepsPerMM;
-   Step.stepsPerMM = 4000;
+   Step.stepsPerMM = 400;
    Step.handleStepper(irqTime);
 
    Obj.StepGenOut2.ActualPosition = Obj.StepGenIn2.CommandedPosition;
@@ -55,7 +55,7 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    Obj.EncFrequency = Encoder1.frequency(ESCvar.Time);
    Obj.IndexByte = Encoder1.getIndexState();
 
-   uint32_t dTim = extendTime(micros()) - irqTime; // thenTime; // Debug. Getting jitter over the last 200 milliseconds
+   uint32_t dTim = longTime.extendTime(micros()) - irqTime; // 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++)
@@ -68,8 +68,11 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    }
    thenTime = irqTime;
    Obj.DiffT = max_Tim - min_Tim; // Debug
-   Obj.DiffT = ccnnt--;
+   Obj.DiffT = abs(Step.nSteps);
-   // Obj.DiffT = Step.frequency;
+   Obj.D1 = Step.Tjitter;
+   Obj.D2 = Step.Tstartf * 1e6;
+   Obj.D3 = Step.dbg;
+   Obj.D4 = Obj.D1+Obj.D2-Obj.D3;
 }
 
 void ESC_interrupt_enable(uint32_t mask);
@@ -127,7 +130,7 @@ void sync0Handler(void)
    ccnnt++;
    ALEventIRQ = ESC_ALeventread();
    serveIRQ = 1;
-   irqTime = extendTime(micros());
+   irqTime = longTime.extendTime(micros());
    digitalWrite(Step.dirPin, cnt++ % 2);
 }
 
@@ -181,24 +184,3 @@ uint16_t dc_checker(void)
    StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
    return 0;
 }
-
-#define ONE_PERIOD UINT32_MAX
-#define HALF_PERIOD (UINT32_MAX >> 1)
-static int64_t previousTimeValue = 0;
-
-// Extend from 32-bit to 64-bit precision
-int64_t extendTime(uint32_t in)
-{
-   int64_t c64 = (int64_t)in - HALF_PERIOD; // remove half period to determine (+/-) sign of the wrap
-   int64_t dif = (c64 - previousTimeValue); // core concept: prev + (current - prev) = current
-
-   // wrap difference from -HALF_PERIOD to HALF_PERIOD. modulo prevents differences after the wrap from having an incorrect result
-   int64_t mod_dif = ((dif + HALF_PERIOD) % ONE_PERIOD) - HALF_PERIOD;
-   if (dif < -HALF_PERIOD)
-      mod_dif += ONE_PERIOD; // account for mod of negative number behavior in C
-
-   int64_t unwrapped = previousTimeValue + mod_dif;
-   previousTimeValue = unwrapped; // load previous value
-
-   return unwrapped + HALF_PERIOD; // remove the shift we applied at the beginning, and return
-}