From 43854ca4d071825cccea01bfb45ee09e5b42e213 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Mon, 5 Feb 2024 21:22:23 +0100 Subject: [PATCH] wip --- Firmware/include/StepGen2.h | 15 +++--- Firmware/src/StepGen2.cpp | 94 ++++++++++++++++++++----------------- Firmware/src/main.cpp | 4 +- 3 files changed, 62 insertions(+), 51 deletions(-) diff --git a/Firmware/include/StepGen2.h b/Firmware/include/StepGen2.h index 2fd3bcb..89a858a 100755 --- a/Firmware/include/StepGen2.h +++ b/Firmware/include/StepGen2.h @@ -6,19 +6,17 @@ class StepGen2 { private: - volatile uint8_t timerIsRunning; - volatile int32_t timerStepPosition; - volatile int32_t timerStepDirection; - volatile int32_t timerStepPositionAtEnd; - volatile int32_t timerNewEndStepPosition; - volatile uint32_t timerNewCycleTime; volatile double_t actualPosition; volatile double_t requestedPosition; volatile double_t oldPosition; volatile int32_t oldStepPosition; volatile uint8_t enabled; + volatile int32_t nSteps; + volatile float Tstart; + volatile float Tstop; + volatile float Tstep; HardwareTimer *MyTim; - HardwareTimer *MyTim2; + HardwareTimer *MyTim2; // 10,11,13,14 int16_t stepsPerMM; uint8_t dirPin; PinName stepPin; @@ -34,10 +32,11 @@ public: static uint32_t sync0CycleTime; volatile uint32_t lcncCycleTime; // Linuxcnc nominal cycle time (1 ms often) - StepGen2(TIM_TypeDef *Timer, TIM_TypeDef *Timer2, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void)); + 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); void timerCB(); + void timer2CB(); void enable(uint8_t yes); void reqPos(double_t pos) { requestedPosition = pos; }; diff --git a/Firmware/src/StepGen2.cpp b/Firmware/src/StepGen2.cpp index ce61eb8..c104406 100755 --- a/Firmware/src/StepGen2.cpp +++ b/Firmware/src/StepGen2.cpp @@ -2,13 +2,8 @@ #include #include "StepGen2.h" -StepGen2::StepGen2(TIM_TypeDef *Timer, TIM_TypeDef *Timer2, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void)) +StepGen2::StepGen2(TIM_TypeDef *Timer, uint32_t _timerChannel, PinName _stepPin, uint8_t _dirPin, void irq(void), TIM_TypeDef *Timer2, void irq2(void)) { - timerIsRunning = 0; - timerStepPosition = 0; - timerStepDirection = 0; - timerStepPositionAtEnd = 0; - timerNewEndStepPosition = 0; actualPosition = 0; requestedPosition = 0; oldPosition = 0; @@ -23,6 +18,7 @@ StepGen2::StepGen2(TIM_TypeDef *Timer, TIM_TypeDef *Timer2, uint32_t _timerChann MyTim->attachInterrupt(irq); pinMode(dirPin, OUTPUT); MyTim2 = new HardwareTimer(Timer2); + MyTim2->attachInterrupt(irq2); } uint32_t StepGen2::handleStepper(void) @@ -31,42 +27,55 @@ uint32_t StepGen2::handleStepper(void) return 1; lcncCycleTime = StepGen2::sync0CycleTime; - float y0TRAJ = oldPos() * getScale(); // Straight line equation between old and new point - float y1TRAJ = reqPos() * getScale(); // Time runs between 0 and lcncCycleTime (1 ms) - float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope - float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime; // Intercept - int32_t stepPosStart = floor(y0TRAJ); // First step position, integer value of first point position - int32_t stepPosStop = floor(y1TRAJ); // End step position - // - float Tstart = (stepPosStart - mTRAJ) / kTRAJ; // First step at this time - float Tstop = (stepPosStop - mTRAJ) / kTRAJ; // And the last step - float Tstep = fabs(1.0 / kTRAJ); // Time between steps - float stepFrequency = fabs(kTRAJ); // 1/Tstep - which is kTRAJ - // - if (Tstart > lcncCycleTime) // Not enough movement to make a step - return updatePosAndReturn(stepPosStop, 2); // - if (/* 1.0 / Tstep */ kTRAJ > 200000) // - { // Too high frequency, deal with this later. - return updatePosAndReturn(stepPosStop, 3); // - } // - int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in - // - if (abs(stepPosStart - oldStepPos()) == 0) // StepPosStart and oldStepPos() are often the same, but don't redo the step - { // - stepPosStart += dir; // New first step - Tstart += Tstep; // - if (Tstart > lcncCycleTime) // Not enough movement to make a step - return updatePosAndReturn(stepPosStop, 4); // - } // - if (abs(stepPosStart - oldStepPos()) > 1) // Shouldn't happen - { // - return updatePosAndReturn(stepPosStop, 5); // - } // - // Now the old point and the start point should be separate. - if (Tstart > lcncCycleTime) // Not enough movement to make a step - return updatePosAndReturn(stepPosStop, 6); // Check this again - // Tstart, Tstep and Tstop defines the coming pwm-sequence. - return 0; // Always do one pulse at Tstart when we come here. Next Tstart+Tstep and so on until Tstop. + float y0TRAJ = oldPos() * getScale(); // Straight line equation between old and new point + float y1TRAJ = reqPos() * getScale(); // Time runs between 0 and lcncCycleTime (1 ms) + float kTRAJ = (y1TRAJ - y0TRAJ) / lcncCycleTime; // Slope + float mTRAJ = y1TRAJ - kTRAJ * lcncCycleTime; // Intercept + int32_t stepPosStart = floor(y0TRAJ); // First step position, integer value of first point position + int32_t stepPosStop = floor(y1TRAJ); // End step position + // + float Tstart = (stepPosStart - mTRAJ) / kTRAJ; // First step at this time + float Tstop = (stepPosStop - mTRAJ) / kTRAJ; // And the last step + float Tstep = fabs(1.0 / kTRAJ); // Time between steps + float stepFrequency = fabs(kTRAJ); // 1/Tstep - which is kTRAJ + // + if (Tstart > lcncCycleTime) // Not enough movement to make a step + return updatePosAndReturn(stepPosStop, 2); // + if (/* 1.0 / Tstep */ kTRAJ > 200000) // + { // Too high frequency, deal with this later. + return updatePosAndReturn(stepPosStop, 3); // + } // + int8_t dir = stepPosStart > stepPosStop ? -1 : 1; // Which direction to step in + // + if (abs(stepPosStart - oldStepPos()) == 0) // StepPosStart and oldStepPos() are often the same, but don't redo the step + { // + stepPosStart += dir; // New first step + Tstart += Tstep; // + if (Tstart > lcncCycleTime) // Not enough movement to make a step + return updatePosAndReturn(stepPosStop, 4); // + } // + if (abs(stepPosStart - oldStepPos()) > 1) // Shouldn't happen + { // + return updatePosAndReturn(stepPosStop, 5); // + } // + // Now the old point and the start point should be separate. + if (Tstart > lcncCycleTime) // Not enough movement to make a step + return updatePosAndReturn(stepPosStop, 6); // Check this again + // Tstart, Tstep and Tstop defines the coming pwm-sequence. + // + MyTim2->setOverflow(Tstart + Tjitter, MICROSEC_FORMAT); // All handled by irqs + MyTim2->resume(); + return updatePosAndReturn(stepPosStop, 0); +} +void StepGen2::timer2CB() +{ + MyTim2->pause(); // Once is enough. + MyTim->setMode(timerChan, TIMER_OUTPUT_COMPARE_PWM2, stepPin); + MyTim->setOverflow(floor(1e6 / Tstep), HERTZ_FORMAT); // 100000 microseconds = 100 milliseconds + MyTim->setCaptureCompare(timerChan, 50, PERCENT_COMPARE_FORMAT); // 50% + nSteps = round((Tstop - Tstart) / Tstep + 1); + if (nSteps > 0) + MyTim->resume(); } void StepGen2::timerCB() { @@ -102,6 +111,7 @@ void StepGen2::timerCB() } #endif } + uint32_t StepGen2::updatePosAndReturn(int32_t stepPosStop, uint32_t i) { // oldPos(reqPos()); // Save the numeric position for next step diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 3aa8e38..fce6759 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -34,8 +34,10 @@ void timerCallbackStep2(void) #endif #include "StepGen2.h" void timerCallbackStep(void); -StepGen2 Step(TIM1, TIM10, 4, PA_11, PA12, timerCallbackStep); +void timerCallbackStepStart(void); +StepGen2 Step(TIM1, 4, PA_11, PA12, timerCallbackStep, TIM10, timerCallbackStepStart); void timerCallbackStep(void) { Step.timerCB(); } +void timerCallbackStepStart(void) { Step.timer2CB(); } CircularBuffer Tim; volatile uint64_t nowTime = 0, thenTime = 0;