From 63fafb4936de9439b45bd14b87eca28ade736517 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Fri, 5 Apr 2024 09:54:43 +0200 Subject: [PATCH] FIx 1c13 index manually, nLoops is back --- Firmware/include/StepGen3.h | 4 ++-- Firmware/lib/soes/objectlist.c | 4 ++-- Firmware/src/StepGen3.cpp | 4 ++-- Firmware/src/main.cpp | 40 +++++++++++++++++----------------- 4 files changed, 26 insertions(+), 26 deletions(-) diff --git a/Firmware/include/StepGen3.h b/Firmware/include/StepGen3.h index 4ed5749..6496306 100755 --- a/Firmware/include/StepGen3.h +++ b/Firmware/include/StepGen3.h @@ -148,8 +148,8 @@ private: // For the example #define BASE_PERIOD 40000 // 40000 is max #define SERVO_PERIOD 1000000 -#define JOINT_X_STEPGEN_MAXACCEL (52000.0) -#define JOINT_Z_STEPGEN_MAXACCEL (52000.0) +#define JOINT_X_STEPGEN_MAXACCEL 0.0 +#define JOINT_Z_STEPGEN_MAXACCEL 0.0 #define JOINT_X_SCALE -200 #define JOINT_Z_SCALE -80 diff --git a/Firmware/lib/soes/objectlist.c b/Firmware/lib/soes/objectlist.c index 05b5f05..91a3f16 100755 --- a/Firmware/lib/soes/objectlist.c +++ b/Firmware/lib/soes/objectlist.c @@ -237,8 +237,8 @@ const _objd SDO1C13[] = {0x07, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_07, 0x1A06, NULL}, {0x08, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_08, 0x1A07, NULL}, {0x09, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_09, 0x1A08, NULL}, - {0x10, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_10, 0x1A09, NULL}, - {0x11, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_11, 0x1A0A, NULL}, + {0x0a, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_10, 0x1A09, NULL}, + {0x0b, DTYPE_UNSIGNED16, 16, ATYPE_RO, acName1C13_11, 0x1A0A, NULL}, }; const _objd SDO6000[] = { diff --git a/Firmware/src/StepGen3.cpp b/Firmware/src/StepGen3.cpp index 1c8abed..5cf807b 100755 --- a/Firmware/src/StepGen3.cpp +++ b/Firmware/src/StepGen3.cpp @@ -1096,9 +1096,9 @@ StepGen3::StepGen3(void) rtapi_app_main(); stepgen_array[0].enable = 1; stepgen_array[0].pos_scale = JOINT_X_SCALE; - //stepgen_array[0].maxaccel = JOINT_X_STEPGEN_MAXACCEL; + stepgen_array[0].maxaccel = JOINT_X_STEPGEN_MAXACCEL; stepgen_array[1].pos_scale = JOINT_Z_SCALE; - //stepgen_array[1].maxaccel = JOINT_Z_STEPGEN_MAXACCEL; + stepgen_array[1].maxaccel = JOINT_Z_STEPGEN_MAXACCEL; stepgen_array[0].enable = stepgen_array[1].enable = 1; } diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 9f10981..119ce70 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -53,6 +53,7 @@ volatile double pos_cmd1, pos_cmd2; void syncWithLCNC() { syncTimer->pause(); + baseTimer->pause(); prevSyncTime = syncTime; syncTime = longTime.extendTime(micros()); deltaSyncTime = syncTime - prevSyncTime; @@ -61,40 +62,34 @@ void syncWithLCNC() Step->updateStepGen(pos_cmd1, pos_cmd2); // Update positions Step->makeAllPulses(); // Make first step right here basePeriodCnt = 1000000 / BASE_PERIOD; // - baseTimer->setCount(0); - baseTimer->refresh(); // - baseTimer->resume(); // Make the other steps in ISR + baseTimer->refresh(); // + baseTimer->resume(); // Make the other steps in ISR // baseTimer->isRunning(); } void basePeriodCB(void) { - uint32_t one = micros(); - - Step->makeAllPulses(); - - isrTime = micros() - one; - if (--basePeriodCnt <= 0) // Stop - { + if (--basePeriodCnt > 0) // Stop + Step->makeAllPulses(); + else baseTimer->pause(); - } } -uint16_t nLoops; -uint64_t reallyNowTime = 0, reallyThenTime = 0; // Times in microseconds -uint64_t timeDiff; // Timediff in microseconds +uint64_t timeDiff; // Timediff in microseconds int32_t delayT; uint16_t avgCycleTime, thisCycleTime; // In usecs int16_t maxCycleTime = 0; volatile uint64_t oldIrqTime = 0; - +uint16_t nLoops; void handleStepper(void) { + if (oldIrqTime != 0) { thisCycleTime = irqTime - oldIrqTime; cycleTimes.add(thisCycleTime); + nLoops = 1 + (irqTime - oldIrqTime) / 960; } oldIrqTime = irqTime; @@ -162,10 +157,11 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation uint16_t newCnt = isrTime; // Obj.D1 = newCnt - oldCnt; oldCnt = newCnt; + Obj.DiffT = nLoops; Obj.D1 = 1000 * Obj.CommandedPosition2; // abs(1000 * (ap2 - Obj.CommandedPosition2)); // Step2.actPos(); Obj.D2 = 1000 * Step->stepgen_array[1].pos_fb; // Step->stepgen_array[1].rawcount % INT16_MAX; // Step->stepgen_array[1].freq; Obj.D3 = Step->stepgen_array[1].freq; - Obj.D4 = Step->stepgen_array[1].rawcount % UINT16_MAX; + Obj.D4 = deltaMakePulsesCnt; // Step->stepgen_array[1].rawcount % UINT16_MAX; } void ESC_interrupt_enable(uint32_t mask); @@ -213,9 +209,10 @@ void setup(void) Step = new StepGen3; - baseTimer = new HardwareTimer(TIM1); // The base period timer + baseTimer = new HardwareTimer(TIM11); // The base period timer uint32_t usecs = BASE_PERIOD / 1000; - setFrequencyAdjustedMicrosSeconds(baseTimer, usecs); + // setFrequencyAdjustedMicrosSeconds(baseTimer, usecs); + baseTimer->setOverflow(20, MICROSEC_FORMAT); baseTimer->attachInterrupt(basePeriodCB); syncTimer = new HardwareTimer(TIM3); // The Linuxcnc servo period sync timer @@ -244,9 +241,12 @@ void loop(void) void sync0Handler(void) { - irqTime = longTime.extendTime(micros()); ALEventIRQ = ESC_ALeventread(); - serveIRQ = 1; + if (ALEventIRQ & ESCREG_ALEVENT_SM2) + { + irqTime = longTime.extendTime(micros()); + serveIRQ = 1; + } irqCnt++; }