From 2b2be4f63debd4c6cf387f94e90faa9a7a893018 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Tue, 13 Feb 2024 10:49:57 +0100 Subject: [PATCH] Going for test in the lathe --- Firmware/include/StepGen2.h | 23 ++++++++++++------ Firmware/src/StepGen2.cpp | 48 +++++++++++++++++++++++++------------ Firmware/src/main.cpp | 36 +++++++--------------------- 3 files changed, 58 insertions(+), 49 deletions(-) diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h index ae0b310..4c78e21 100755 --- a/Firmware/include/StepGen2.h +++ b/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 uint32_t Tstartu; // Starting delay in usecs -private: -public: + volatile float Tstartf; // Starting delay in secs + volatile uint32_t Tstartu; // Starting delay in usecs + volatile float Tpulses; // Time it takes to do pulses. Debug 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 \ No newline at end of file diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp index dbdb3a2..63dedba 100755 --- a/Firmware/src/StepGen2.cpp +++ b/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 - { // - Tstartf = 0; // Just take a step in the middle of the cycle - frequency = 1000 * (abs(nSteps) + 1); // At some suitable frequency + + if (abs(nSteps) < 1000) // Some small number + { // + 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); - nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction. + frequency = fabs(kTRAJ * stepsPerMM); // + Tpulses = abs(nSteps) / frequency; } updatePos(5); - uint64_t nowTime = extendTime(micros()); // usecs - dbg = nowTime - irqTime; - Tstartu = Tjitter + Tstartf * 1e6 // Was secs, now usecs - - (nowTime - irqTime); // Have already wasted some time since the irq. + 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. + 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->setCaptureCompare(pulseTimerChan, 50, PERCENT_COMPARE_FORMAT); // 50% + pulseTimer->setOverflow(timerFrequency, HERTZ_FORMAT); + 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 +} \ No newline at end of file diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index b9ff56a..13c6829 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -27,7 +27,7 @@ void startTimerCallback(void) { Step.startTimerCB(); } CircularBuffer 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 = Step.frequency; + Obj.DiffT = abs(Step.nSteps); + 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 -}