diff --git a/Firmware/doc/Stepgen.odp b/Firmware/doc/Stepgen.odp index 572e8ba..8d0384a 100755 Binary files a/Firmware/doc/Stepgen.odp and b/Firmware/doc/Stepgen.odp differ diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h index b40505c..ae0b310 100755 --- a/Firmware/include/StepGen2.h +++ b/Firmware/include/StepGen2.h @@ -21,7 +21,8 @@ public: HardwareTimer *startTimer; // 10,11,13,14 uint8_t dirPin; PinName stepPin; - const float Tjitter = 5.0; // Time unit is microseconds + const float Tjitter = 500.0; // Time unit is microseconds + uint64_t dbg; public: volatile double_t commandedPosition; // End position when this cycle is completed @@ -37,7 +38,7 @@ 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(void); + uint32_t handleStepper(uint64_t irqTime/* time for irq nanosecs */); void startTimerCB(); void pulseTimerCB(); uint32_t updatePos(uint32_t i); diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp index 637925d..dbdb3a2 100755 --- a/Firmware/src/StepGen2.cpp +++ b/Firmware/src/StepGen2.cpp @@ -1,6 +1,11 @@ #include #include #include "StepGen2.h" +extern "C" +{ +#include "esc.h" +} +extern int64_t extendTime(uint32_t); StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void)) { @@ -21,10 +26,10 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, startTimer = new HardwareTimer(Timer2); startTimer->attachInterrupt(irq2); } -uint32_t cnt = 0; -uint32_t StepGen2::handleStepper(void) +extern volatile uint32_t cnt; +uint32_t StepGen2::handleStepper(uint64_t irqTime) { - + digitalWrite(dirPin, cnt++ % 2); if (!enabled) return updatePos(0); @@ -32,39 +37,44 @@ uint32_t StepGen2::handleStepper(void) commandedStepPosition = floor(commandedPosition * stepsPerMM); // Scale position to steps if (initialStepPosition == commandedStepPosition) // No movement return 1; -// digitalWrite(dirPin, cnt++ % 2); + float approximateFrequency = fabs(initialStepPosition - commandedStepPosition) // We must take at least one step / lcncCycleTime; // from here on - // if (approximateFrequency > maxAllowedFrequency) // Stay on this position - // return 1; + // if (approximateFrequency > maxAllowedFrequency) // Stay on this position + // return 1; float kTRAJ = (commandedPosition - initialPosition) / lcncCycleTime; // Straight line equation float mTRAJ = initialPosition; // position = kTRAJ x time + mTRAJ // Operating on incoming positions (not steps) - if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01) // Very flat slope + // if (fabs(kTRAJ * lcncCycleTime * stepsPerMM) < 0.01) // Very flat slope + nSteps = commandedStepPosition - initialStepPosition; // + if (abs(nSteps) <= 8) // Some small number { // - Tstartf = 0.5 * lcncCycleTime; // Just take a step in the middle of the cycle - frequency = 10000; // At some suitable frequency - nSteps = kTRAJ > 0 ? 1 : -1; // Take only one step + Tstartf = 0; // Just take a step in the middle of the cycle + frequency = 1000 * (abs(nSteps) + 1); // At some suitable frequency } else // Regular step train, up or down { if (kTRAJ > 0) - Tstartf = (ceil(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ; + Tstartf = (float(initialStepPosition + 1) / float(stepsPerMM) - mTRAJ) / kTRAJ; else - Tstartf = (floor(initialPosition * stepsPerMM) / stepsPerMM - mTRAJ) / kTRAJ; + Tstartf = (float(initialStepPosition) / float(stepsPerMM) - mTRAJ) / kTRAJ; frequency = fabs(kTRAJ * stepsPerMM); nSteps = commandedStepPosition - initialStepPosition; // sign(nSteps) = direction. } updatePos(5); - Tstartu = Tstartf * 1e6; // Was secs, now usecs + 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. - startTimer->setOverflow(Tstartu + Tjitter, MICROSEC_FORMAT); // All handled by irqs + startTimer->setOverflow(Tstartu, MICROSEC_FORMAT); // All handled by irqs startTimer->resume(); return 1; } void StepGen2::startTimerCB() { + digitalWrite(dirPin, cnt++ % 2); startTimer->pause(); // Once is enough. // digitalWrite(dirPin, nSteps > 0 ? 1 : -1); timerPulseSteps = abs(nSteps); @@ -77,7 +87,10 @@ void StepGen2::pulseTimerCB() { --timerPulseSteps; if (timerPulseSteps == 0) + { pulseTimer->pause(); + digitalWrite(dirPin, cnt++ % 2); + } } uint32_t StepGen2::updatePos(uint32_t i) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 0ecba70..b9ff56a 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -26,6 +26,7 @@ void pulseTimerCallback(void) { Step.pulseTimerCB(); } 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 void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation @@ -42,7 +43,7 @@ void handleStepper(void) Obj.StepGenOut1.ActualPosition = Step.commandedPosition; Step.stepsPerMM = Obj.StepGenIn1.StepsPerMM; Step.stepsPerMM = 4000; - Step.handleStepper(); + Step.handleStepper(irqTime); Obj.StepGenOut2.ActualPosition = Obj.StepGenIn2.CommandedPosition; } @@ -54,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 = irqTime - thenTime; // Debug. Getting jitter over the last 200 milliseconds + uint32_t dTim = 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++) @@ -67,8 +68,8 @@ void cb_get_inputs(void) // Set Master inputs, slave outputs, last operation } thenTime = irqTime; Obj.DiffT = max_Tim - min_Tim; // Debug - Obj.DiffT = ALEventIRQ; - //Obj.DiffT = Step.frequency; + Obj.DiffT = ccnnt--; + // Obj.DiffT = Step.frequency; } void ESC_interrupt_enable(uint32_t mask); @@ -112,7 +113,7 @@ void loop(void) { CC_ATOMIC_SET(ESCvar.ALevent, ESC_ALeventread()); DIG_process(ALEventIRQ, DIG_PROCESS_WD_FLAG | DIG_PROCESS_OUTPUTS_FLAG | - DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); + DIG_PROCESS_APP_HOOK_FLAG | DIG_PROCESS_INPUTS_FLAG); serveIRQ = 0; ESCvar.PrevTime = ESCvar.Time; } @@ -120,14 +121,17 @@ void loop(void) if ((dTime > 200 && dTime < 500) || dTime > 1500) // Don't run ecat_slv_poll when expecting to serve interrupt ecat_slv_poll(); } - +volatile uint32_t cnt = 0; void sync0Handler(void) { - irqTime = micros(); - serveIRQ = 1; + ccnnt++; ALEventIRQ = ESC_ALeventread(); + serveIRQ = 1; + irqTime = extendTime(micros()); + digitalWrite(Step.dirPin, cnt++ % 2); } +// Enable SM2 interrupts void ESC_interrupt_enable(uint32_t mask) { // Enable interrupt for SYNC0 or SM2 or SM3 @@ -135,9 +139,8 @@ void ESC_interrupt_enable(uint32_t mask) uint32_t user_int_mask = ESCREG_ALEVENT_SM2; // Only SM2 if (mask & user_int_mask) { - ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); - ESC_ALeventmaskwrite(ESC_ALeventmaskread() | (mask & user_int_mask)); + ESC_ALeventmaskwrite(ESC_ALeventmaskread() & ~(ESCREG_ALEVENT_DC_SYNC0 | ESCREG_ALEVENT_SM3)); attachInterrupt(digitalPinToInterrupt(PC3), sync0Handler, RISING); // Set LAN9252 interrupt pin driver as push-pull active high @@ -150,6 +153,7 @@ void ESC_interrupt_enable(uint32_t mask) } } +// Disable SM2 interrupts void ESC_interrupt_disable(uint32_t mask) { // Enable interrupt for SYNC0 or SM2 or SM3 @@ -182,7 +186,8 @@ uint16_t dc_checker(void) #define HALF_PERIOD (UINT32_MAX >> 1) static int64_t previousTimeValue = 0; -int64_t extendTime(uint32_t in) // Extend from 32-bit to 64-bit precision +// 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