From 120b423f59cf39d17c931069338515176da242f0 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Mon, 8 Jan 2024 21:13:34 +0100 Subject: [PATCH 1/3] Restructure, makes pulses again --- Firmware/src/main.cpp | 61 ++++++++++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 21 deletions(-) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 26b11e3..30215fe 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -36,10 +36,7 @@ volatile uint32_t stepCount = 0, stepPulses = 0; volatile double_t actualPosition = 0; volatile double_t requestedPosition, requestedVelocity; -uint32_t sync0CycleTime = 0; - -void handleStepper(void); -void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/); +uint32_t sync0CycleTime = 0; // nanoseconds void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { @@ -98,7 +95,7 @@ uint16_t dc_checker(void); void TimerStep_CB(void); void sync0Handler(void); void handleStepper(void); -void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/); +void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* nr of pulses to do*/); static esc_cfg_t config = { @@ -108,7 +105,7 @@ static esc_cfg_t config = .set_defaults_hook = NULL, .pre_state_change_hook = NULL, .post_state_change_hook = NULL, - .application_hook = handleStepper, + .application_hook = handleStepper, .safeoutput_override = NULL, .pre_object_download_hook = NULL, .post_object_download_hook = NULL, @@ -173,32 +170,54 @@ void sync0Handler(void) serveIRQ = 1; } +volatile uint8_t timerIsRunning = 0; +volatile uint8_t reloadStepperTimer = 0; +volatile int32_t currentPosition = 0; +volatile int32_t direction = 1; +volatile int32_t timerStepPosition = 0; +volatile int32_t timerStepDirection = 0; +volatile int32_t timerStepPositionAtEnd = 0; + void handleStepper(void) { - int32_t pulsesToGo = 5000 * (requestedPosition - actualPosition); // From Turner.hal X:5000 Z:2000 ps/mm - if (pulsesToGo != 0) - makePulses(sync0CycleTime, pulsesToGo); // Make the pulses using hardware timer - + int32_t pulsesAtEndOfCycle = 1000 * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm + makePulses(/*sync0CycleTime / 1000*/ 700, pulsesAtEndOfCycle); // Make the pulses using hardware timer actualPosition = requestedPosition; } -void makePulses(uint32_t period /* in usecs */, int32_t pulses /* nr of pulses to do*/) +void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/) { - byte sgn = pulses > 0 ? HIGH : LOW; - digitalWrite(STEPPER_DIR_PIN, sgn); // I think one should really wait a bit when changed - uint32_t puls = abs(pulses); - MyTim->setOverflow(abs(pulses) * 1000000 / period, HERTZ_FORMAT); - MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % - stepCount = 0; - stepPulses = abs(pulses); - MyTim->resume(); + // if (!stepperTimerIsRunning) + { + int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or - + if (abs(steps) * 1000000 / cycleTime > 100000) // 100 kHz is too much for driver, reduce + { + int32_t stepsMax = 100000 * cycleTime / 1000000; + steps = stepsMax*(steps > 0 ? 1 : -1); + pulsesAtEnd = timerStepPositionAtEnd + stepsMax; + } + if (steps != 0) + { + byte sgn = steps > 0 ? HIGH : LOW; + digitalWrite(STEPPER_DIR_PIN, sgn); + uint64_t period = cycleTime; // usecs + uint32_t freq = abs(steps) * 1000000 / period; + MyTim->setOverflow(freq, HERTZ_FORMAT); + MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % + timerStepDirection = steps > 0 ? 1 : -1; + timerStepPositionAtEnd = pulsesAtEnd; // Current Position + timerIsRunning = 1; + MyTim->resume(); + } + } } void TimerStep_CB(void) { - stepCount++; - if (stepCount == stepPulses) + timerStepPosition += timerStepDirection; + if (timerStepPosition == timerStepPositionAtEnd) { + timerIsRunning = 0; MyTim->pause(); } } From 0b9ce37200552fd56191342edd525b949a18026e Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Tue, 9 Jan 2024 10:39:56 +0100 Subject: [PATCH 2/3] Reloading in irq might possibly work --- Firmware/src/main.cpp | 58 +++++++++++++++++++++++++++++++++---------- 1 file changed, 45 insertions(+), 13 deletions(-) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 30215fe..34e4000 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -180,28 +180,32 @@ volatile int32_t timerStepPositionAtEnd = 0; void handleStepper(void) { - int32_t pulsesAtEndOfCycle = 1000 * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm - makePulses(/*sync0CycleTime / 1000*/ 700, pulsesAtEndOfCycle); // Make the pulses using hardware timer + int32_t pulsesAtEndOfCycle = 100 * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm + makePulses(/*sync0CycleTime / 1000*/ 1200, pulsesAtEndOfCycle); // Make the pulses using hardware timer actualPosition = requestedPosition; } +volatile int32_t timerNewEndStepPosition = 0; +volatile uint64_t timerNewEndTime = 0; + void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/) { - // if (!stepperTimerIsRunning) + if (1 /*!timerIsRunning*/) { + // Start the timer int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or - - if (abs(steps) * 1000000 / cycleTime > 100000) // 100 kHz is too much for driver, reduce - { - int32_t stepsMax = 100000 * cycleTime / 1000000; - steps = stepsMax*(steps > 0 ? 1 : -1); - pulsesAtEnd = timerStepPositionAtEnd + stepsMax; - } if (steps != 0) { + if (abs(steps) * 1000000 / cycleTime > 100000) // 100 kHz is too much for driver, reduce + { + int32_t stepsMax = 100000 * cycleTime / 1000000; + steps = stepsMax * (steps > 0 ? 1 : -1); + pulsesAtEnd = timerStepPositionAtEnd + steps; + } byte sgn = steps > 0 ? HIGH : LOW; digitalWrite(STEPPER_DIR_PIN, sgn); - uint64_t period = cycleTime; // usecs - uint32_t freq = abs(steps) * 1000000 / period; + uint32_t freq = 1.4 * abs(steps) * 1000000 / cycleTime; + MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); MyTim->setOverflow(freq, HERTZ_FORMAT); MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % timerStepDirection = steps > 0 ? 1 : -1; @@ -210,12 +214,40 @@ void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end po MyTim->resume(); } } + else // Timer is running, reload + { + // Set variables, they will be picked up by the timer_CB and the timer is reloaded. + + timerNewEndStepPosition = pulsesAtEnd; + timerNewEndTime = micros() + cycleTime; + } } void TimerStep_CB(void) { - timerStepPosition += timerStepDirection; - if (timerStepPosition == timerStepPositionAtEnd) + timerStepPosition += timerStepDirection; // The step that was just completed + if (timerNewEndTime != 0) // Are we going to reload? + { + // Input for reload is timerNewEndStepPosition and timerNewEndTime + // The timer has current position and current time and from this + // can set new frequency and new endtarget for steps + MyTim->pause(); + int32_t steps = timerNewEndStepPosition - timerStepPosition; + uint64_t cycleTime = timerNewEndTime - micros(); + byte sgn = steps > 0 ? HIGH : LOW; + digitalWrite(STEPPER_DIR_PIN, sgn); + uint32_t freq = abs(steps) * 1000000 / cycleTime; + MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); + MyTim->setOverflow(freq, HERTZ_FORMAT); + MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % + timerStepDirection = steps > 0 ? 1 : -1; + timerStepPositionAtEnd = timerNewEndStepPosition; + timerNewEndStepPosition = 0; // Set to zero to not reload next time + timerNewEndTime = 0; + timerIsRunning = 1; + MyTim->resume(); + } + if (timerStepPosition == timerStepPositionAtEnd) // Are we finished? { timerIsRunning = 0; MyTim->pause(); From 9adae08b981e8f0be85b0799e6a86aa6ba29b790 Mon Sep 17 00:00:00 2001 From: Hakan Bastedt Date: Tue, 9 Jan 2024 22:20:20 +0100 Subject: [PATCH 3/3] It seems to work now, with reload in the timer_CB. Avoid micros() --- Firmware/src/main.cpp | 82 ++++++++++++++++++++++++------------------- 1 file changed, 46 insertions(+), 36 deletions(-) diff --git a/Firmware/src/main.cpp b/Firmware/src/main.cpp index 34e4000..27a2187 100755 --- a/Firmware/src/main.cpp +++ b/Firmware/src/main.cpp @@ -36,7 +36,7 @@ volatile uint32_t stepCount = 0, stepPulses = 0; volatile double_t actualPosition = 0; volatile double_t requestedPosition, requestedVelocity; -uint32_t sync0CycleTime = 0; // nanoseconds +uint32_t sync0CycleTime = 0; // microseconds void cb_set_outputs(void) // Master outputs gets here, slave inputs, first operation { @@ -180,72 +180,82 @@ volatile int32_t timerStepPositionAtEnd = 0; void handleStepper(void) { - int32_t pulsesAtEndOfCycle = 100 * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm - makePulses(/*sync0CycleTime / 1000*/ 1200, pulsesAtEndOfCycle); // Make the pulses using hardware timer - actualPosition = requestedPosition; + const uint32_t steps_per_mm = 1000; + actualPosition = timerStepPosition / double(steps_per_mm); + double diffPosition = requestedPosition - actualPosition; + if (abs(diffPosition) * steps_per_mm > 10000) + { + requestedPosition = actualPosition + 10.0 * (diffPosition > 0 ? 1 : -1); + } + int32_t pulsesAtEndOfCycle = steps_per_mm * requestedPosition; // From Turner.hal X:5000 Z:2000 ps/mm + makePulses(sync0CycleTime, pulsesAtEndOfCycle); // Make the pulses using hardware timer } volatile int32_t timerNewEndStepPosition = 0; -volatile uint64_t timerNewEndTime = 0; +volatile uint32_t timerNewCycleTime = 0; void makePulses(uint64_t cycleTime /* in usecs */, int32_t pulsesAtEnd /* end position*/) { - if (1 /*!timerIsRunning*/) + uint32_t now = micros(); + if (timerIsRunning) + { + // Set variables, they will be picked up by the timer_CB and the timer is reloaded. + timerNewEndStepPosition = pulsesAtEnd; + timerNewCycleTime = cycleTime; + } + if (!timerIsRunning) { // Start the timer int32_t steps = pulsesAtEnd - timerStepPositionAtEnd; // Pulses to go + or - if (steps != 0) { - if (abs(steps) * 1000000 / cycleTime > 100000) // 100 kHz is too much for driver, reduce - { - int32_t stepsMax = 100000 * cycleTime / 1000000; - steps = stepsMax * (steps > 0 ? 1 : -1); - pulsesAtEnd = timerStepPositionAtEnd + steps; - } byte sgn = steps > 0 ? HIGH : LOW; digitalWrite(STEPPER_DIR_PIN, sgn); - uint32_t freq = 1.4 * abs(steps) * 1000000 / cycleTime; - MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); + double_t freqf = (abs(steps) * 1000000.0) / double(cycleTime); + uint32_t freq = uint32_t(freqf); + // freq=1428; MyTim->setOverflow(freq, HERTZ_FORMAT); MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % timerStepDirection = steps > 0 ? 1 : -1; timerStepPositionAtEnd = pulsesAtEnd; // Current Position timerIsRunning = 1; + MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); MyTim->resume(); } } - else // Timer is running, reload - { - // Set variables, they will be picked up by the timer_CB and the timer is reloaded. - - timerNewEndStepPosition = pulsesAtEnd; - timerNewEndTime = micros() + cycleTime; - } } void TimerStep_CB(void) { timerStepPosition += timerStepDirection; // The step that was just completed - if (timerNewEndTime != 0) // Are we going to reload? + if (timerNewCycleTime != 0) // Are we going to reload? { // Input for reload is timerNewEndStepPosition and timerNewEndTime // The timer has current position and current time and from this // can set new frequency and new endtarget for steps MyTim->pause(); int32_t steps = timerNewEndStepPosition - timerStepPosition; - uint64_t cycleTime = timerNewEndTime - micros(); - byte sgn = steps > 0 ? HIGH : LOW; - digitalWrite(STEPPER_DIR_PIN, sgn); - uint32_t freq = abs(steps) * 1000000 / cycleTime; - MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); - MyTim->setOverflow(freq, HERTZ_FORMAT); - MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % - timerStepDirection = steps > 0 ? 1 : -1; - timerStepPositionAtEnd = timerNewEndStepPosition; - timerNewEndStepPosition = 0; // Set to zero to not reload next time - timerNewEndTime = 0; - timerIsRunning = 1; - MyTim->resume(); + if (steps != 0) + { + byte sgn = steps > 0 ? HIGH : LOW; + digitalWrite(STEPPER_DIR_PIN, sgn); + double_t freqf = (abs(steps) * 1000000.0) / double(timerNewCycleTime); + uint32_t freq = uint32_t(freqf); + // freq=1428; + if (freq != 0) + { + MyTim->setMode(4, TIMER_OUTPUT_COMPARE_PWM2, STEPPER_STEP_PIN); + // freq=1428; + MyTim->setOverflow(freq, HERTZ_FORMAT); + MyTim->setCaptureCompare(4, 50, PERCENT_COMPARE_FORMAT); // 50 % + timerStepDirection = steps > 0 ? 1 : -1; + timerStepPositionAtEnd = timerNewEndStepPosition; + timerNewEndStepPosition = 0; // Set to zero to not reload next time + timerNewCycleTime = 0; + MyTim->resume(); + timerIsRunning = 1; + } + } } if (timerStepPosition == timerStepPositionAtEnd) // Are we finished? { @@ -299,7 +309,7 @@ uint16_t dc_checker(void) { // Indicate we run DC ESCvar.dcsync = 0; - sync0CycleTime = ESC_SYNC0cycletime(); + sync0CycleTime = ESC_SYNC0cycletime() / 1000; // nsec to usec return 0; } #define ONE_PERIOD 65536