From ec1c8fc70f66455472fdb970375fa6a8dfe760eb Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Sun, 7 Apr 2024 23:27:10 +0200 Subject: [PATCH] Enable encoder again. Cleanup of unnecessary variables --- Firmware/src/main.cpp | 69 ++++++++++--------------------------------- 1 file changed, 15 insertions(+), 54 deletions(-) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index cdb8b0b..f3e8a53 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -22,28 +22,22 @@ void indexPulseEncoderCB1(void) Encoder1.indexPulse(); } #include -RunningAverage irqServeDelays(1000); // To have a running average of the irq serve delay over the last second +RunningAverage irqServeDelays(1000); // To get the max delay of the irq serve time over the last second #include "StepGen3.h" StepGen3 *Step = 0; -CircularBuffer Tim; - #include "extend32to64.h" -volatile uint64_t irqTime = 0, thenTime = 0, nowTime = 0, irqCnt = 0, prevSyncTime = 0, syncTime = 0, deltaSyncTime; +volatile uint64_t irqTime = 0, irqCnt = 0; extend32to64 longTime; void setFrequencyAdjustedMicrosSeconds(HardwareTimer *timer, uint32_t usecs); void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { - // Encoder1.setLatch(Obj.IndexLatchEnable); - // Encoder1.setScale(2000); - - // Step2.reqPos(Obj.CommandedPosition2); - // Step2.setScale(Obj.StepsPerMM2); - // Step2.enable(1); + Encoder1.setLatch(Obj.IndexLatchEnable); + Encoder1.setScale(2000); } volatile uint16_t basePeriodCnt; @@ -55,9 +49,6 @@ void syncWithLCNC() { syncTimer->pause(); baseTimer->pause(); - prevSyncTime = syncTime; - syncTime = longTime.extendTime(micros()); - deltaSyncTime = syncTime - prevSyncTime; deltaMakePulsesCnt = makePulsesCnt - prevMakePulsesCnt; prevMakePulsesCnt = makePulsesCnt; Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions @@ -65,7 +56,6 @@ void syncWithLCNC() basePeriodCnt = SERVO_PERIOD / BASE_PERIOD; // baseTimer->refresh(); // baseTimer->resume(); // Make the other steps in ISR - // baseTimer->isRunning(); } void basePeriodCB(void) @@ -76,13 +66,12 @@ void basePeriodCB(void) baseTimer->pause(); } -uint64_t timeDiff; // Timediff in microseconds int32_t delayT; -uint16_t avgCycleTime, thisCycleTime; // In usecs +uint16_t thisCycleTime; // In usecs int16_t maxIrqServeTime = 0; - -volatile uint64_t oldIrqTime = 0; +uint64_t oldIrqTime = 0; uint16_t nLoops; + void handleStepper(void) { if (oldIrqTime != 0) // See if there is a delay in data, normally it *should* be nLoops=1, but sometimes it is more @@ -113,7 +102,6 @@ void handleStepper(void) // Obj.ActualPosition1 = Step->stepgen_array[0].pos_fb; // Obj.ActualPosition2 = Step->stepgen_array[1].pos_fb; - // maxIrqServeTime = 600; delayT = maxIrqServeTime - diffT; // Add 10 as some safety margin if (delayT > 0 && delayT < 900) { @@ -126,38 +114,15 @@ void handleStepper(void) syncWithLCNC(); } } -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(); -// Obj.EncPos = Encoder1.currentPos(); -// Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); -// Obj.IndexByte = Encoder1.getIndexState(); -#if 0 - float_t ap2 = Obj.ActualPosition2; + Obj.IndexStatus = Encoder1.indexHappened(); + Obj.EncPos = Encoder1.currentPos(); + Obj.EncFrequency = Encoder1.frequency(ESCvar.Time); + Obj.IndexByte = Encoder1.getIndexState(); - uint64_t dTim = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds - Tim.push(dTim); - uint64_t max_Tim = 0, min_Tim = UINT64_MAX; - for (decltype(Tim)::index_t i = 0; i < Tim.size(); i++) - { - uint32_t aTim = Tim[i]; - if (aTim > max_Tim) - max_Tim = aTim; - if (aTim < min_Tim) - min_Tim = aTim; - } - thenTime = irqTime; -#endif - if (irqCnt == 1000) - startTime = irqTime; - if (irqCnt == 11000) - avgTime = (irqTime - startTime) / 1000; - - // 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; @@ -201,7 +166,7 @@ void setup(void) #if 0 measureCrystalFrequency(); // Calibrate crystal frequency #endif - // rcc_config(); // Needed by encoder, probably breaks some timers. + rcc_config(); // Needed by encoder, probably breaks some timers. ecat_slv_init(&config); pinMode(PA11, OUTPUT); // Step X @@ -213,7 +178,7 @@ void setup(void) baseTimer = new HardwareTimer(TIM11); // The base period timer setFrequencyAdjustedMicrosSeconds(baseTimer, BASE_PERIOD / 1000); - // baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); + // baseTimer->setOverflow(BASE_PERIOD / 1000, MICROSEC_FORMAT); // Or the line above, This one is uncalibrated baseTimer->attachInterrupt(basePeriodCB); syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer @@ -225,10 +190,6 @@ void loop(void) uint64_t dTime; if (serveIRQ) { - nowTime = longTime.extendTime(micros()); - /* Read local time from ESC*/ - // ESC_read(ESCREG_LOCALTIME, (void *)&ESCvar.Time, sizeof(ESCvar.Time)); - // ESCvar.Time = etohl(ESCvar.Time); DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); serveIRQ = 0; @@ -248,7 +209,7 @@ void sync0Handler(void) irqTime = longTime.extendTime(micros()); serveIRQ = 1; } - irqCnt++; + irqCnt++; // debug output } // Enable SM2 interrupts