From fef934b103b16629f54c49e534db7c55cb00f1b3 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sun, 10 Mar 2024 01:03:50 +0100 Subject: [PATCH] wip --- Firmware/include/StepGen2.h | 10 +++++----- Firmware/src/StepGen2.cpp | 17 ++++++++++------- Firmware/src/main.cpp | 35 ++++++++++++++++++++++++----------- 3 files changed, 39 insertions(+), 23 deletions(-) diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h index b69345a..5f11c32 100755 --- a/Firmware/include/StepGen2.h +++ b/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 - volatile float lcncCycleTime; // Linuxcnc nominal cycle time in sec (1 ms often) + static uint32_t sync0CycleTime; // Nominal EtherCAT cycle time nanoseconds + 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); diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp index 2904764..5befadc 100755 --- a/Firmware/src/StepGen2.cpp +++ b/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; - Tstartu = Tjitter + uint32_t(Tstartf * 1e6) - timeSinceISR; // Have already wasted some time since the irq. + dbg = timeSinceISR; // + 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; diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index bbbe8d8..ab9fea1 100755 --- a/Firmware/src/main.cpp +++ b/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 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 = Step1.Tjitter; - Obj.D2 = Step1.Tstartf * 1e6; - Obj.D3 = Step1.dbg; + Obj.D1 = Step2.frequency; + Obj.D2 = Step2.Tstartf * 1e6; + Obj.D3 = Step2.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; }