wip

Firmware/include/StepGen2.h

@@ -18,10 +18,10 @@ public:
 
     HardwareTimer *pulseTimer;
     uint32_t pulseTimerChan;
-    HardwareTimer *startTimer; // 10,11,13,14
+    HardwareTimer *startTimer; // Use timers 10,11,13,14
     uint8_t dirPin;
     PinName stepPin;
-    const uint32_t Tjitter = 500; // Time unit is microseconds
+    uint32_t Tjitter = 400; // Longest time from IRQ to handling in handleStepper, unit is microseconds
     uint64_t dbg;
     const uint16_t t2 = 5;                                            // DIR is ahead of PUL with at least 5 usecs
     const uint16_t t3 = 3;                                            // Pulse width at least 2.5 usecs
@@ -37,12 +37,12 @@ public:
     volatile uint8_t enabled;               // Enabled step generator
     volatile float frequency;
 
-    static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time
+    static uint32_t sync0CycleTime;     // Nominal EtherCAT cycle time nanoseconds
-    volatile float lcncCycleTime;   // Linuxcnc nominal cycle time in sec (1 ms often)
+    volatile float lcncCycleTime;       // Linuxcnc nominal cycle time in sec (1 ms often)
 
     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 when irq happened nanosecs */, uint16_t nLoops);
     void startTimerCB();
     void pulseTimerCB();
     uint32_t updatePos(uint32_t i);

Firmware/src/StepGen2.cpp

@@ -26,20 +26,20 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin,
     startTimer = new HardwareTimer(Timer2);
     startTimer->attachInterrupt(irq2);
 }
-extern volatile uint32_t cnt;
+
-uint32_t StepGen2::handleStepper(uint64_t irqTime)
+uint32_t StepGen2::handleStepper(uint64_t irqTime, uint16_t nLoops)
 {
     if (!enabled)
         return updatePos(0);
 
-    lcncCycleTime = StepGen2::sync0CycleTime * 1.0e-6;             // was usec, became sec
+    lcncCycleTime = nLoops * StepGen2::sync0CycleTime * 1.0e-9;    // // nLoops is there in case we missed a ethercat cycle. secs
     commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps
     if (initialStepPosition == commandedStepPosition)              // No movement
         return updatePos(1);
 
     nSteps = commandedStepPosition - initialStepPosition;
 
-    if (abs(nSteps) < 2) // Some small number
+    if (abs(nSteps) < 1) // Some small number
     {
         frequency = (abs(nSteps) + 1) / lcncCycleTime;
         Tpulses = abs(nSteps) / frequency;
@@ -58,14 +58,15 @@ uint32_t StepGen2::handleStepper(uint64_t irqTime)
     }
     updatePos(5);
     uint32_t timeSinceISR = (longTime.extendTime(micros()) - irqTime); // Diff time from ISR (usecs)
-    dbg = timeSinceISR;
+    dbg = timeSinceISR;                                                //
-    Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq.
+    Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR;        // Have already wasted some time since the irq.
 
     timerFrequency = uint32_t(ceil(frequency));
     startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs
     startTimer->resume();
     return 1;
 }
+
 void StepGen2::startTimerCB()
 {
     startTimer->pause(); // Once is enough.
@@ -74,9 +75,11 @@ void StepGen2::startTimerCB()
     timerPulseSteps = abs(nSteps);
     pulseTimer->setMode(pulseTimerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin);
     pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT);
-    pulseTimer->setCaptureCompare(pulseTimerChan, 5, MICROSEC_COMPARE_FORMAT); // 5 usecs
+    // pulseTimer->setCaptureCompare(pulseTimerChan, t3, MICROSEC_COMPARE_FORMAT);
+    pulseTimer->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT);
     pulseTimer->resume();
 }
+
 void StepGen2::pulseTimerCB()
 {
     --timerPulseSteps;

Firmware/src/main.cpp

@@ -19,14 +19,14 @@ void indexPulseEncoderCB1(void)
 }
 
 #include "StepGen2.h"
-//Stepper 1
+// Stepper 1
 void pulseTimerCallback1(void);
 void startTimerCallback1(void);
 StepGen2 Step1(TIM1, 4, PA_11, PA12, pulseTimerCallback1, TIM10, startTimerCallback1);
 void pulseTimerCallback1(void) { Step1.pulseTimerCB(); }
 void startTimerCallback1(void) { Step1.startTimerCB(); }
 
-//Stepper 2
+// Stepper 2
 void pulseTimerCallback2(void);
 void startTimerCallback2(void);
 StepGen2 Step2(TIM3, 4, PC_9, PC10, pulseTimerCallback2, TIM11, startTimerCallback2);
@@ -35,6 +35,8 @@ void startTimerCallback2(void) { Step2.startTimerCB(); }
 
 CircularBuffer<uint32_t, 200> Tim;
 volatile uint64_t irqTime = 0, thenTime = 0, nowTime = 0;
+volatile uint64_t EcatTimeIRQ = 0, EcatTimeThen = 0, EcatTimeDiff = 0;
+;
 volatile uint32_t ccnnt = 0;
 extend32to64 longTime;
 
@@ -49,19 +51,29 @@ void cb_set_outputs(void) // Master outputs gets here, slave inputs, first opera
    Obj.ActualPosition1 = Obj.CommandedPosition1; // Step1.actPos();
    Obj.ActualPosition2 = Obj.CommandedPosition2; // Step2.actPos();
 }
-volatile uint32_t cmt;
+
+uint16_t nLoops;
 void handleStepper(void)
 {
-   //digitalWrite(Step.dirPin, cmt++ % 2);
+   if (!(ALEventIRQ & ESCREG_ALEVENT_SM2))
+      return;
+   // Catch the case when we miss a loop for some reason
+   uint64_t EcatTimeNow;
+   ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeNow, sizeof(EcatTimeNow));
+   EcatTimeNow = etohl(EcatTimeNow);
+   EcatTimeDiff = EcatTimeNow - EcatTimeThen;
+   nLoops = round(EcatTimeDiff / double(StepGen2::sync0CycleTime));
+   EcatTimeThen = EcatTimeNow;
+
    Step1.enabled = true;
    Step1.commandedPosition = Obj.CommandedPosition1;
    Step1.stepsPerMM = Obj.StepsPerMM1;
-   Step1.handleStepper(irqTime);
+   Step1.handleStepper(irqTime, nLoops);
 
    Step2.enabled = true;
    Step2.commandedPosition = Obj.CommandedPosition2;
    Step2.stepsPerMM = Obj.StepsPerMM2;
-   Step2.handleStepper(irqTime);
+   Step2.handleStepper(irqTime, nLoops);
 }
 
 void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
@@ -84,10 +96,9 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation
    }
    thenTime = irqTime;
    Obj.DiffT = max_Tim - min_Tim; // Debug
-   Obj.DiffT = abs(Step1.nSteps);
+   Obj.D1 = Step2.frequency;
-   Obj.D1 = Step1.Tjitter;
+   Obj.D2 = Step2.Tstartf * 1e6;
-   Obj.D2 = Step1.Tstartf * 1e6;
+   Obj.D3 = Step2.dbg;
-   Obj.D3 = Step1.dbg;
    Obj.D4 = Obj.D1 + Obj.D2 - Obj.D3;
 }
 
@@ -149,6 +160,8 @@ void sync0Handler(void)
    ccnnt++;
    ALEventIRQ = ESC_ALeventread();
    serveIRQ = 1;
+   ESC_read(ESCREG_LOCALTIME, (void *)&EcatTimeIRQ, sizeof(EcatTimeIRQ));
+   EcatTimeIRQ = etohl(EcatTimeIRQ);
    irqTime = longTime.extendTime(micros());
 }
 
@@ -199,6 +212,6 @@ uint16_t dc_checker(void)
 {
    // Indicate we run DC
    ESCvar.dcsync = 1;
-   StepGen2::sync0CycleTime = ESC_SYNC0cycletime() / 1000; // usecs
+   StepGen2::sync0CycleTime = ESC_SYNC0cycletime(); // nsecs
    return 0;
 }