From e7a6c77d74e7bb80a11350ff1ebef4653646e2c5 Mon Sep 17 00:00:00 2001 From: InsanityAutomation Date: Tue, 7 Jun 2022 09:11:41 -0400 Subject: [PATCH 01/19] Resolve DUE Servo pulse issue --- Marlin/src/HAL/DUE/Servo.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5524aa9cef..f479ea2463 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -71,12 +71,15 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); #endif void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { - // clear interrupt - tc->TC_CHANNEL[channel].TC_SR; if (Channel[timer] < 0) tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) + else { + //else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated + } + + // clear interrupt + tc->TC_CHANNEL[channel].TC_SR; Channel[timer]++; // increment to the next channel if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { @@ -92,6 +95,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel } + } static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { From 6566031ec6b15063fa057b4b9abc8a968fedaac6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 17:08:45 -0500 Subject: [PATCH 02/19] keep check for good SERVO_INDEX --- Marlin/src/HAL/AVR/Servo.cpp | 28 ++++++++++++++-------------- Marlin/src/HAL/DUE/Servo.cpp | 29 ++++++++++++++--------------- 2 files changed, 28 insertions(+), 29 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 526352b773..f5359b394b 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -67,26 +67,26 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s /************ static functions common to all instances ***********************/ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { + const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) - *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer - else { - if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated - } + *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer + else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated - Channel[timer]++; // increment to the next channel - if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + Channel[timer]++; // increment to the next channel + if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { *OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks; - if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high } else { - // finished all channels so wait for the refresh period to expire before starting over - if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed - *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL); + // Finished all channels so wait for the refresh period to expire before starting over + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); + if (((unsigned)*TCNTn) + 4 < iticks) // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = iticks; else - *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed - Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed + Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } } diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index f479ea2463..5d75eb6985 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -71,29 +71,28 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); #endif void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { + const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) - tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else { - //else if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && SERVO(timer, Channel[timer]).Pin.isActive) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated - } + tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer + else if (good_servo) + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated - // clear interrupt - tc->TC_CHANNEL[channel].TC_SR; + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt - Channel[timer]++; // increment to the next channel - if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && Channel[timer] < SERVOS_PER_TIMER) { + Channel[timer]++; // increment to the next channel + if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; - if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // its an active channel so pulse it high + if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high } else { // finished all channels so wait for the refresh period to expire before starting over + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); tc->TC_CHANNEL[channel].TC_RA = - tc->TC_CHANNEL[channel].TC_CV < usToTicks(REFRESH_INTERVAL) - 4 - ? (unsigned int)usToTicks(REFRESH_INTERVAL) // allow a few ticks to ensure the next OCR1A not missed - : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed - Channel[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel + tc->TC_CHANNEL[channel].TC_CV + 4 < iticks + ? iticks // allow a few ticks to ensure the next OCR1A not missed + : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed + Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } } From 86b9e7ee1324b504a11452297031c85730700e2b Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 17:37:47 -0500 Subject: [PATCH 03/19] servo cleanup --- Marlin/src/HAL/AVR/Servo.cpp | 20 ++++++++-------- Marlin/src/HAL/DUE/Servo.cpp | 33 +++++++++++---------------- Marlin/src/HAL/SAMD51/Servo.cpp | 4 ++-- Marlin/src/HAL/STM32F1/Servo.cpp | 16 ++++++------- Marlin/src/HAL/shared/servo.cpp | 10 ++++---- Marlin/src/HAL/shared/servo_private.h | 12 +++++----- 6 files changed, 44 insertions(+), 51 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index f5359b394b..379eb08215 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -66,7 +66,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s /************ static functions common to all instances ***********************/ -static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { +static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer @@ -81,11 +81,9 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t } else { // Finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); - if (((unsigned)*TCNTn) + 4 < iticks) // allow a few ticks to ensure the next OCR1A not missed - *OCRnA = iticks; - else - *OCRnA = *TCNTn + 4; // at least REFRESH_INTERVAL has elapsed + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed + tctick = ((unsigned)*TCNTn) + 4; // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = max(tctick, iticks); Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } } @@ -123,7 +121,7 @@ static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t /****************** end of static functions ******************************/ -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 if (timer == _timer1) { TCCR1A = 0; // normal counting mode @@ -182,7 +180,7 @@ void initISR(timer16_Sequence_t timer) { #endif } -void finISR(timer16_Sequence_t timer) { +void finISR(const timer16_Sequence_t timer) { // Disable use of the given timer #ifdef WIRING if (timer == _timer1) { @@ -192,7 +190,8 @@ void finISR(timer16_Sequence_t timer) { #else TIMSK #endif - , OCIE1A); // disable timer 1 output compare interrupt + , OCIE1A // disable timer 1 output compare interrupt + ); timerDetach(TIMER1OUTCOMPAREA_INT); } else if (timer == _timer3) { @@ -202,7 +201,8 @@ void finISR(timer16_Sequence_t timer) { #else ETIMSK #endif - , OCIE3A); // disable the timer3 output compare A interrupt + , OCIE3A // disable the timer3 output compare A interrupt + ); timerDetach(TIMER3OUTCOMPAREA_INT); } #else // !WIRING diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5d75eb6985..69e2cc1b2b 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -52,7 +52,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s // ------------------------ /// Interrupt handler for the TC0 channel 1. // ------------------------ -void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); +void Servo_Handler(const timer16_Sequence_t timer, Tc *pTc, const uint8_t channel); #ifdef _useTimer1 void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } @@ -70,7 +70,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); } #endif -void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { +void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer @@ -82,16 +82,14 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { Channel[timer]++; // increment to the next channel if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; - if (SERVO(timer,Channel[timer]).Pin.isActive) // check if activated + if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL); - tc->TC_CHANNEL[channel].TC_RA = - tc->TC_CHANNEL[channel].TC_CV + 4 < iticks - ? iticks // allow a few ticks to ensure the next OCR1A not missed - : tc->TC_CHANNEL[channel].TC_CV + 4; // at least REFRESH_INTERVAL has elapsed + const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed + tctick = tc->TC_CHANNEL[channel].TC_CV + 4; // allow a few ticks to ensure the next OCR1A not missed + tc->TC_CHANNEL[channel].TC_RA = max(tctick, iticks); Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel } @@ -116,30 +114,25 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_Start(tc, channel); } -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 - if (timer == _timer1) - _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); + if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); #endif #ifdef _useTimer2 - if (timer == _timer2) - _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); + if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); #endif #ifdef _useTimer3 - if (timer == _timer3) - _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); + if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); #endif #ifdef _useTimer4 - if (timer == _timer4) - _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); + if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); #endif #ifdef _useTimer5 - if (timer == _timer5) - _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); + if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); #endif } -void finISR(timer16_Sequence_t) { +void finISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); #endif diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 23ab21c615..04461653f9 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -124,7 +124,7 @@ HAL_SERVO_TIMER_ISR() { } } -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); @@ -201,7 +201,7 @@ void initISR(timer16_Sequence_t timer) { } } -void finISR(timer16_Sequence_t timer) { +void finISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); diff --git a/Marlin/src/HAL/STM32F1/Servo.cpp b/Marlin/src/HAL/STM32F1/Servo.cpp index 8dc1ef7b6a..47ffb631cf 100644 --- a/Marlin/src/HAL/STM32F1/Servo.cpp +++ b/Marlin/src/HAL/STM32F1/Servo.cpp @@ -147,17 +147,17 @@ void libServo::move(const int32_t value) { uint16_t SR = timer_get_status(tdev); if (SR & TIMER_SR_CC1IF) { // channel 1 off #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 1); // off + OUT_WRITE_OD(SERVO0_PIN, HIGH); // off #else - OUT_WRITE(SERVO0_PIN, 0); + OUT_WRITE(SERVO0_PIN, LOW); #endif timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); } if (SR & TIMER_SR_CC2IF) { // channel 2 resume #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 0); // on + OUT_WRITE_OD(SERVO0_PIN, LOW); // on #else - OUT_WRITE(SERVO0_PIN, 1); + OUT_WRITE(SERVO0_PIN, HIGH); #endif timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); } @@ -167,9 +167,9 @@ void libServo::move(const int32_t value) { timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); if (!tdev) return false; #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(inPin, 1); + OUT_WRITE_OD(inPin, HIGH); #else - OUT_WRITE(inPin, 0); + OUT_WRITE(inPin, LOW); #endif timer_pause(tdev); @@ -200,9 +200,9 @@ void libServo::move(const int32_t value) { timer_disable_irq(tdev, 1); timer_disable_irq(tdev, 2); #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(pin, 1); // off + OUT_WRITE_OD(pin, HIGH); // off #else - OUT_WRITE(pin, 0); + OUT_WRITE(pin, LOW); #endif } } diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index cfec6f3017..567ff81598 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -65,7 +65,7 @@ uint8_t ServoCount = 0; // the total number of attached /************ static functions common to all instances ***********************/ -static boolean isTimerActive(timer16_Sequence_t timer) { +static bool anyTimerChannelActive(const timer16_Sequence_t timer) { // returns true if any servo is active on this timer LOOP_L_N(channel, SERVOS_PER_TIMER) { if (SERVO(timer, channel).Pin.isActive) @@ -102,16 +102,16 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { // initialize the timer if it has not already been initialized timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) initISR(timer); - servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + if (!anyTimerChannelActive(timer)) initISR(timer); + servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive return servoIndex; } void Servo::detach() { servo_info[servoIndex].Pin.isActive = false; - timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) finISR(timer); + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (anyTimerChannelActive(timer)) finISR(timer); } void Servo::write(int value) { diff --git a/Marlin/src/HAL/shared/servo_private.h b/Marlin/src/HAL/shared/servo_private.h index d85d8da8ba..021e0cb81d 100644 --- a/Marlin/src/HAL/shared/servo_private.h +++ b/Marlin/src/HAL/shared/servo_private.h @@ -70,10 +70,10 @@ #define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond()) // convenience macros -#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo -#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer -#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel -#define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel +#define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel +#define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel // Types @@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS]; // Public functions -extern void initISR(timer16_Sequence_t timer); -extern void finISR(timer16_Sequence_t timer); +extern void initISR(const timer16_Sequence_t timer); +extern void finISR(const timer16_Sequence_t timer); From 6c3928662615bbf3b7061937971376e81e470014 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 17:39:39 -0500 Subject: [PATCH 04/19] Try Servo::detach adjustment ?? --- Marlin/src/HAL/DUE/Servo.cpp | 2 +- Marlin/src/HAL/shared/servo.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 69e2cc1b2b..7fda31f13e 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -74,7 +74,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo) + else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated tc->TC_CHANNEL[channel].TC_SR; // clear interrupt diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index 567ff81598..87bb9a73fb 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -109,9 +109,9 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { } void Servo::detach() { - servo_info[servoIndex].Pin.isActive = false; const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); if (anyTimerChannelActive(timer)) finISR(timer); + servo_info[servoIndex].Pin.isActive = false; } void Servo::write(int value) { From 04ffe9493bb262aacb1945901c635ab1b25c8a7c Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 17:39:58 -0500 Subject: [PATCH 05/19] Try adjusting finISR also ?? --- Marlin/src/HAL/DUE/Servo.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 7fda31f13e..c527de01d9 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -134,19 +134,19 @@ void initISR(const timer16_Sequence_t timer) { void finISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 - TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); + if (timer == _timer1) TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); #endif #ifdef _useTimer2 - TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); + if (timer == _timer2) TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); #endif #ifdef _useTimer3 - TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); + if (timer == _timer3) TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); #endif #ifdef _useTimer4 - TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); + if (timer == _timer4) TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); #endif #ifdef _useTimer5 - TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); + if (timer == _timer5) TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); #endif } From 7f6622e6acd58b9fe1b7695ae31fdf223f143360 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 18:13:12 -0500 Subject: [PATCH 06/19] test this if nothing else works --- Marlin/src/HAL/shared/servo.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index 87bb9a73fb..4e424aa81e 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -112,6 +112,7 @@ void Servo::detach() { const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); if (anyTimerChannelActive(timer)) finISR(timer); servo_info[servoIndex].Pin.isActive = false; + //extDigitalWrite(servo_info[servoIndex].Pin.nbr, LOW); // The ISR won't be called, so set the pin LOW here } void Servo::write(int value) { From dbd932138005af306ae9c9795bd14b0d1ce63372 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 22:28:44 -0500 Subject: [PATCH 07/19] use simple fix --- Marlin/src/HAL/AVR/Servo.cpp | 4 ++-- Marlin/src/HAL/DUE/Servo.cpp | 4 ++-- Marlin/src/HAL/shared/servo.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 379eb08215..676cfd04db 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -70,8 +70,8 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated + else if (good_servo) + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // always pulse the channel LOW Channel[timer]++; // increment to the next channel if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index c527de01d9..a04f0e4aa7 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -74,8 +74,8 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // pulse this channel low if activated + else if (good_servo) + extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // always pulse the channel low tc->TC_CHANNEL[channel].TC_SR; // clear interrupt diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index 4e424aa81e..baf4246fc2 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -109,10 +109,10 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { } void Servo::detach() { + servo_info[servoIndex].Pin.isActive = false; const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); if (anyTimerChannelActive(timer)) finISR(timer); - servo_info[servoIndex].Pin.isActive = false; - //extDigitalWrite(servo_info[servoIndex].Pin.nbr, LOW); // The ISR won't be called, so set the pin LOW here + pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input } void Servo::write(int value) { From c1775c9005e5315adfd8a81cfee901b12189b2e9 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Tue, 7 Jun 2022 22:29:17 -0500 Subject: [PATCH 08/19] Remove MOVE_SERVO, DETACH_SERVO --- Marlin/src/feature/bltouch.cpp | 2 +- Marlin/src/feature/spindle_laser.cpp | 4 ++-- Marlin/src/gcode/config/M43.cpp | 8 ++++---- Marlin/src/gcode/control/M280.cpp | 6 +++--- Marlin/src/gcode/control/M282.cpp | 2 +- Marlin/src/module/probe.cpp | 2 +- Marlin/src/module/servo.cpp | 8 ++++---- Marlin/src/module/servo.h | 7 ++----- Marlin/src/module/temperature.cpp | 6 +++--- Marlin/src/module/tool_change.cpp | 8 ++++---- 10 files changed, 25 insertions(+), 28 deletions(-) diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp index d911fae6ae..10d3131aed 100644 --- a/Marlin/src/feature/bltouch.cpp +++ b/Marlin/src/feature/bltouch.cpp @@ -45,7 +45,7 @@ void stop(); bool BLTouch::command(const BLTCommand cmd, const millis_t &ms) { if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("BLTouch Command :", cmd); - MOVE_SERVO(Z_PROBE_SERVO_NR, cmd); + servo[Z_PROBE_SERVO_NR].move(cmd); safe_delay(_MAX(ms, (uint32_t)BLTOUCH_DELAY)); // BLTOUCH_DELAY is also the *minimum* delay return triggered(); } diff --git a/Marlin/src/feature/spindle_laser.cpp b/Marlin/src/feature/spindle_laser.cpp index 8f70816c99..4f8f4d49dc 100644 --- a/Marlin/src/feature/spindle_laser.cpp +++ b/Marlin/src/feature/spindle_laser.cpp @@ -58,7 +58,7 @@ cutter_power_t SpindleLaser::menuPower, // Power s */ void SpindleLaser::init() { #if ENABLED(SPINDLE_SERVO) - MOVE_SERVO(SPINDLE_SERVO_NR, SPINDLE_SERVO_MIN); + servo[SPINDLE_SERVO_NR].move(SPINDLE_SERVO_MIN); #else OUT_WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE); // Init spindle to off #endif @@ -131,7 +131,7 @@ void SpindleLaser::apply_power(const uint8_t opwr) { isReady = false; } #elif ENABLED(SPINDLE_SERVO) - MOVE_SERVO(SPINDLE_SERVO_NR, power); + servo[SPINDLE_SERVO_NR].move(power); #else WRITE(SPINDLE_LASER_ENA_PIN, enabled() ? SPINDLE_LASER_ACTIVE_STATE : !SPINDLE_LASER_ACTIVE_STATE); isReady = true; diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp index de76b8e979..688b94c9bf 100644 --- a/Marlin/src/gcode/config/M43.cpp +++ b/Marlin/src/gcode/config/M43.cpp @@ -198,10 +198,10 @@ inline void servo_probe_test() { uint8_t i = 0; SERIAL_ECHOLNPGM(". Deploy & stow 4 times"); do { - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy safe_delay(500); deploy_state = READ(PROBE_TEST_PIN); - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow safe_delay(500); stow_state = READ(PROBE_TEST_PIN); } while (++i < 4); @@ -226,7 +226,7 @@ inline void servo_probe_test() { } // Ask the user for a trigger event and measure the pulse width. - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy safe_delay(500); SERIAL_ECHOLNPGM("** Please trigger probe within 30 sec **"); uint16_t probe_counter = 0; @@ -256,7 +256,7 @@ inline void servo_probe_test() { } else SERIAL_ECHOLNPGM("FAIL: Noise detected - please re-run test"); - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow return; } } diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp index ed156e16e9..82981e44bc 100644 --- a/Marlin/src/gcode/control/M280.cpp +++ b/Marlin/src/gcode/control/M280.cpp @@ -56,14 +56,14 @@ void GcodeSuite::M280() { while (PENDING(now, end)) { safe_delay(50); now = _MIN(millis(), end); - MOVE_SERVO(servo_index, LROUND(aold + (anew - aold) * (float(now - start) / t))); + servo[servo_index].move(LROUND(aold + (anew - aold) * (float(now - start) / t))); } } #endif // POLARGRAPH - MOVE_SERVO(servo_index, anew); + servo[servo_index].move(anew); } else - DETACH_SERVO(servo_index); + servo[servo_index].detach(); } else SERIAL_ECHO_MSG(" Servo ", servo_index, ": ", servo[servo_index].read()); diff --git a/Marlin/src/gcode/control/M282.cpp b/Marlin/src/gcode/control/M282.cpp index e6f5ce7dcc..3ac5ac9f5b 100644 --- a/Marlin/src/gcode/control/M282.cpp +++ b/Marlin/src/gcode/control/M282.cpp @@ -36,7 +36,7 @@ void GcodeSuite::M282() { const int servo_index = parser.value_int(); if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) - DETACH_SERVO(servo_index); + servo[servo_index].detach(); else SERIAL_ECHO_MSG("Servo ", servo_index, " out of range"); diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index 66e1c85028..2649aa41b6 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -343,7 +343,7 @@ FORCE_INLINE void probe_specific_action(const bool deploy) { #elif HAS_Z_SERVO_PROBE - MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]); + servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]); #elif EITHER(TOUCH_MI_PROBE, Z_PROBE_ALLEN_KEY) diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp index 96d5ba9da8..2782be1f2b 100644 --- a/Marlin/src/module/servo.cpp +++ b/Marlin/src/module/servo.cpp @@ -39,19 +39,19 @@ hal_servo_t servo[NUM_SERVOS]; void servo_init() { #if NUM_SERVOS >= 1 && HAS_SERVO_0 servo[0].attach(SERVO0_PIN); - DETACH_SERVO(0); // Just set up the pin. We don't have a position yet. Don't move to a random position. + servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position. #endif #if NUM_SERVOS >= 2 && HAS_SERVO_1 servo[1].attach(SERVO1_PIN); - DETACH_SERVO(1); + servo[1].detach(); #endif #if NUM_SERVOS >= 3 && HAS_SERVO_2 servo[2].attach(SERVO2_PIN); - DETACH_SERVO(2); + servo[2].detach(); #endif #if NUM_SERVOS >= 4 && HAS_SERVO_3 servo[3].attach(SERVO3_PIN); - DETACH_SERVO(3); + servo[3].detach(); #endif } diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h index cd55a317a2..2ed992aa03 100644 --- a/Marlin/src/module/servo.h +++ b/Marlin/src/module/servo.h @@ -103,14 +103,11 @@ }; #if HAS_Z_SERVO_PROBE - #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][0]) - #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1]) + #define DEPLOY_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][0]) + #define STOW_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][1]) #endif #endif // HAS_SERVO_ANGLES -#define MOVE_SERVO(I, P) servo[I].move(P) -#define DETACH_SERVO(I) servo[I].detach() - extern hal_servo_t servo[NUM_SERVOS]; void servo_init(); diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index 531c52c09d..17ab08d4c2 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -1881,7 +1881,7 @@ void Temperature::manage_heater() { #endif #if ENABLED(CHAMBER_VENT) flag_chamber_excess_heat = false; - MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 90); + servo[CHAMBER_VENT_SERVO_NR].move(90); #endif } #endif @@ -1897,7 +1897,7 @@ void Temperature::manage_heater() { if (flag_chamber_excess_heat) { temp_chamber.soft_pwm_amount = 0; #if ENABLED(CHAMBER_VENT) - if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, temp_chamber.celsius <= temp_chamber.target ? 0 : 90); + if (!flag_chamber_off) servo[CHAMBER_VENT_SERVO_NR].move(temp_chamber.celsius <= temp_chamber.target ? 0 : 90); #endif } else { @@ -1910,7 +1910,7 @@ void Temperature::manage_heater() { temp_chamber.soft_pwm_amount = temp_chamber.celsius < temp_chamber.target ? (MAX_CHAMBER_POWER) >> 1 : 0; #endif #if ENABLED(CHAMBER_VENT) - if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 0); + if (!flag_chamber_off) servo[CHAMBER_VENT_SERVO_NR].move(0); #endif } } diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index e2f1443048..4b292c92f9 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -116,7 +116,7 @@ void move_extruder_servo(const uint8_t e) { planner.synchronize(); if ((EXTRUDERS & 1) && e < EXTRUDERS - 1) { - MOVE_SERVO(_SERVO_NR(e), servo_angles[_SERVO_NR(e)][e & 1]); + servo[_SERVO_NR(e)].move(servo_angles[_SERVO_NR(e)][e & 1]); safe_delay(500); } } @@ -131,7 +131,7 @@ constexpr int8_t sns_index[2] = { SWITCHING_NOZZLE_SERVO_NR, SWITCHING_NOZZLE_E1_SERVO_NR }; constexpr int16_t sns_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES; planner.synchronize(); - MOVE_SERVO(sns_index[e], sns_angles[angle_index]); + servo[sns_index[e]].move(sns_angles[angle_index]); safe_delay(500); } @@ -142,7 +142,7 @@ void move_nozzle_servo(const uint8_t angle_index) { planner.synchronize(); - MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_NOZZLE_SERVO_NR][angle_index]); + servo[SWITCHING_NOZZLE_SERVO_NR].move(servo_angles[SWITCHING_NOZZLE_SERVO_NR][angle_index]); safe_delay(500); } @@ -443,7 +443,7 @@ void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0. inline void switching_toolhead_lock(const bool locked) { #ifdef SWITCHING_TOOLHEAD_SERVO_ANGLES const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES; - MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]); + servo[SWITCHING_TOOLHEAD_SERVO_NR].move(swt_angles[locked ? 0 : 1]); #elif PIN_EXISTS(SWT_SOLENOID) OUT_WRITE(SWT_SOLENOID_PIN, locked); gcode.dwell(10); From fb5dd4094910ff3a8f4f81f4e54567fbda8e9bf6 Mon Sep 17 00:00:00 2001 From: InsanityAutomation Date: Wed, 8 Jun 2022 17:38:04 -0400 Subject: [PATCH 09/19] Revise timer to MCK/2 Co-Authored-By: sjasonsmith <20053467+sjasonsmith@users.noreply.github.com> --- Marlin/src/HAL/DUE/Servo.cpp | 7 ++++--- Marlin/src/HAL/DUE/ServoTimers.h | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index a04f0e4aa7..5b63637dbc 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -71,13 +71,14 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *pTc, const uint8_t channe #endif void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt + const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; if (Channel[timer] < 0) tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo) + else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // always pulse the channel low - tc->TC_CHANNEL[channel].TC_SR; // clear interrupt Channel[timer]++; // increment to the next channel if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { @@ -98,7 +99,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { pmc_enable_periph_clk(id); TC_Configure(tc, channel, - TC_CMR_TCCLKS_TIMER_CLOCK3 | // MCK/32 + TC_CMR_TCCLKS_TIMER_CLOCK1 | // MCK/32 TC_CMR_WAVE | // Waveform mode TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC diff --git a/Marlin/src/HAL/DUE/ServoTimers.h b/Marlin/src/HAL/DUE/ServoTimers.h index c32c938253..f95afeda41 100644 --- a/Marlin/src/HAL/DUE/ServoTimers.h +++ b/Marlin/src/HAL/DUE/ServoTimers.h @@ -37,7 +37,7 @@ #define _useTimer5 #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays -#define SERVO_TIMER_PRESCALER 32 // timer prescaler +#define SERVO_TIMER_PRESCALER 2 // timer prescaler /* TC0, chan 0 => TC0_Handler From 0206af6c079ed39317a0118204588f1926139e84 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 Jun 2022 00:13:04 -0500 Subject: [PATCH 10/19] fix servo index check, add comments --- Marlin/src/HAL/AVR/Servo.cpp | 31 ++++++++++---------- Marlin/src/HAL/DUE/Servo.cpp | 50 +++++++++++++++----------------- Marlin/src/HAL/DUE/ServoTimers.h | 2 +- Marlin/src/HAL/SAMD51/Servo.cpp | 25 ++++++++-------- Marlin/src/HAL/shared/servo.cpp | 6 ++-- 5 files changed, 57 insertions(+), 57 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 676cfd04db..b3f65a36b2 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -67,24 +67,25 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s /************ static functions common to all instances ***********************/ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { - const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; - if (Channel[timer] < 0) - *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // always pulse the channel LOW + int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first + if (cho < 0) // Channel -1 indicates the refresh interval completed... + *TCNTn = 0; // ...so reset the timer + else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel ... handled and active? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW - Channel[timer]++; // increment to the next channel - if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { - *OCRnA = *TCNTn + SERVO(timer, Channel[timer]).ticks; - if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + Channel[timer] = ++cho; // Handle the next channel (or 0) + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { + *OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration + if (SERVO(timer, cho).Pin.isActive) // check if activated + extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // if active, set HIGH. (set LOW next interrupt) } else { - // Finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed - tctick = ((unsigned)*TCNTn) + 4; // allow a few ticks to ensure the next OCR1A not missed - *OCRnA = max(tctick, iticks); - Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel + // finished all channels so wait for the refresh period to expire before starting over + const unsigned int tval = ((unsigned)*TCNTn) + 4, // at least REFRESH_INTERVAL has elapsed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed + *OCRnA = max(tval, ival); + + Channel[timer] = -1; // reset the timer counter to 0 on the next call } } diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5b63637dbc..fd6ae1d79a 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -52,7 +52,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s // ------------------------ /// Interrupt handler for the TC0 channel 1. // ------------------------ -void Servo_Handler(const timer16_Sequence_t timer, Tc *pTc, const uint8_t channel); +void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t); #ifdef _useTimer1 void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } @@ -71,35 +71,33 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *pTc, const uint8_t channe #endif void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { - tc->TC_CHANNEL[channel].TC_SR; // clear interrupt + tc->TC_CHANNEL[channel].TC_SR; // clear interrupt + int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first + if (cho < 0) // Channel -1 indicates the refresh interval completed... + tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer + else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel... handled and active? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW - const bool good_servo = SERVO_INDEX(timer, Channel[timer]) < ServoCount; - if (Channel[timer] < 0) - tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // channel set to -1 indicated that refresh interval completed so reset the timer - else if (good_servo && SERVO(timer, Channel[timer]).Pin.isActive) - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, LOW); // always pulse the channel low - - - Channel[timer]++; // increment to the next channel - if (good_servo && Channel[timer] < SERVOS_PER_TIMER) { - tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer,Channel[timer]).ticks; - if (SERVO(timer, Channel[timer]).Pin.isActive) // check if activated - extDigitalWrite(SERVO(timer, Channel[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + Channel[timer] = ++cho; // go to the next channel (or 0) + if (SERVO_INDEX(timer, cho) < ServoCount && cho < SERVOS_PER_TIMER) { + tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks; + if (SERVO(timer, cho).Pin.isActive) // activated? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int iticks = (unsigned int)usToTicks(REFRESH_INTERVAL), // At least the refresh interval has elapsed - tctick = tc->TC_CHANNEL[channel].TC_CV + 4; // allow a few ticks to ensure the next OCR1A not missed - tc->TC_CHANNEL[channel].TC_RA = max(tctick, iticks); - Channel[timer] = -1; // gets incremented at the end of the refresh period to start again at the first channel - } + const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 4, // at least REFRESH_INTERVAL has elapsed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed + tc->TC_CHANNEL[channel].TC_RA = max(cval, ival); + Channel[timer] = -1; // reset the timer CCR on the next call + } } static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { pmc_enable_periph_clk(id); TC_Configure(tc, channel, - TC_CMR_TCCLKS_TIMER_CLOCK1 | // MCK/32 + TC_CMR_TCCLKS_TIMER_CLOCK1 | // MCK/2 TC_CMR_WAVE | // Waveform mode TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC @@ -133,21 +131,21 @@ void initISR(const timer16_Sequence_t timer) { #endif } -void finISR(const timer16_Sequence_t timer) { +void finISR(timer16_Sequence_t) { #ifdef _useTimer1 - if (timer == _timer1) TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); + TC_Stop(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); #endif #ifdef _useTimer2 - if (timer == _timer2) TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); + TC_Stop(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2); #endif #ifdef _useTimer3 - if (timer == _timer3) TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); + TC_Stop(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3); #endif #ifdef _useTimer4 - if (timer == _timer4) TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); + TC_Stop(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4); #endif #ifdef _useTimer5 - if (timer == _timer5) TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); + TC_Stop(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); #endif } diff --git a/Marlin/src/HAL/DUE/ServoTimers.h b/Marlin/src/HAL/DUE/ServoTimers.h index f95afeda41..95bd404c80 100644 --- a/Marlin/src/HAL/DUE/ServoTimers.h +++ b/Marlin/src/HAL/DUE/ServoTimers.h @@ -37,7 +37,7 @@ #define _useTimer5 #define TRIM_DURATION 2 // compensation ticks to trim adjust for digitalWrite delays -#define SERVO_TIMER_PRESCALER 2 // timer prescaler +#define SERVO_TIMER_PRESCALER 2 // timer prescaler /* TC0, chan 0 => TC0_Handler diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 04461653f9..dfe188fde3 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -77,7 +77,8 @@ HAL_SERVO_TIMER_ISR() { ; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); - if (currentServoIndex[timer] < 0) { + int8_t cho = currentServoIndex[timer]; + if (cho < 0) { #if defined(_useTimer1) && defined(_useTimer2) if (currentServoIndex[timer ^ 1] >= 0) { // Wait for both channels @@ -89,28 +90,28 @@ HAL_SERVO_TIMER_ISR() { tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT); } - else if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && SERVO(timer, currentServoIndex[timer]).Pin.isActive) - digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, LOW); // pulse this channel low if activated + else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) + digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse this channel low if activated // Select the next servo controlled by this timer - currentServoIndex[timer]++; + currentServoIndex[timer] = ++cho; - if (SERVO_INDEX(timer, currentServoIndex[timer]) < ServoCount && currentServoIndex[timer] < SERVOS_PER_TIMER) { - if (SERVO(timer, currentServoIndex[timer]).Pin.isActive) // check if activated - digitalWrite(SERVO(timer, currentServoIndex[timer]).Pin.nbr, HIGH); // it's an active channel so pulse it high + if (SERVO_INDEX(timer, cho) < ServoCount && cho < SERVOS_PER_TIMER) { + if (SERVO(timer, cho).Pin.isActive) // check if activated + digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // it's an active channel so pulse it high - tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, currentServoIndex[timer]).ticks; + tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks; } else { // finished all channels so wait for the refresh period to expire before starting over currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel - const uint16_t tcCounterValue = getTimerCount(); + const uint16_t tcCounterValue = getTimerCount(), ival = (uint16_t)usToTicks(REFRESH_INTERVAL); - if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < usToTicks(REFRESH_INTERVAL)) // allow a few ticks to ensure the next OCR1A not missed - tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - (uint16_t)usToTicks(REFRESH_INTERVAL); + if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < ival) // allow a few ticks to ensure the next OCR1A not missed + tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - ival; else - tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed + tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed } if (tcChannel == 0) { SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index baf4246fc2..b838800de6 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -101,7 +101,7 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { max = (MAX_PULSE_WIDTH - inMax) / 4; // initialize the timer if it has not already been initialized - timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); if (!anyTimerChannelActive(timer)) initISR(timer); servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive @@ -111,8 +111,8 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { void Servo::detach() { servo_info[servoIndex].Pin.isActive = false; const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (anyTimerChannelActive(timer)) finISR(timer); - pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input + if (!anyTimerChannelActive(timer)) finISR(timer); + //pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input } void Servo::write(int value) { From e4e6abe5af0f97ff5cb0c9334cb298d3584968e3 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 Jun 2022 00:51:08 -0500 Subject: [PATCH 11/19] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20Remov?= =?UTF-8?q?e=20servo=20macros?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/feature/bltouch.cpp | 2 +- Marlin/src/feature/spindle_laser.cpp | 4 ++-- Marlin/src/gcode/config/M43.cpp | 8 ++++---- Marlin/src/gcode/control/M280.cpp | 6 +++--- Marlin/src/gcode/control/M282.cpp | 2 +- Marlin/src/module/probe.cpp | 2 +- Marlin/src/module/servo.cpp | 8 ++++---- Marlin/src/module/servo.h | 7 ++----- Marlin/src/module/temperature.cpp | 6 +++--- Marlin/src/module/tool_change.cpp | 8 ++++---- 10 files changed, 25 insertions(+), 28 deletions(-) diff --git a/Marlin/src/feature/bltouch.cpp b/Marlin/src/feature/bltouch.cpp index d911fae6ae..10d3131aed 100644 --- a/Marlin/src/feature/bltouch.cpp +++ b/Marlin/src/feature/bltouch.cpp @@ -45,7 +45,7 @@ void stop(); bool BLTouch::command(const BLTCommand cmd, const millis_t &ms) { if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("BLTouch Command :", cmd); - MOVE_SERVO(Z_PROBE_SERVO_NR, cmd); + servo[Z_PROBE_SERVO_NR].move(cmd); safe_delay(_MAX(ms, (uint32_t)BLTOUCH_DELAY)); // BLTOUCH_DELAY is also the *minimum* delay return triggered(); } diff --git a/Marlin/src/feature/spindle_laser.cpp b/Marlin/src/feature/spindle_laser.cpp index 8f70816c99..4f8f4d49dc 100644 --- a/Marlin/src/feature/spindle_laser.cpp +++ b/Marlin/src/feature/spindle_laser.cpp @@ -58,7 +58,7 @@ cutter_power_t SpindleLaser::menuPower, // Power s */ void SpindleLaser::init() { #if ENABLED(SPINDLE_SERVO) - MOVE_SERVO(SPINDLE_SERVO_NR, SPINDLE_SERVO_MIN); + servo[SPINDLE_SERVO_NR].move(SPINDLE_SERVO_MIN); #else OUT_WRITE(SPINDLE_LASER_ENA_PIN, !SPINDLE_LASER_ACTIVE_STATE); // Init spindle to off #endif @@ -131,7 +131,7 @@ void SpindleLaser::apply_power(const uint8_t opwr) { isReady = false; } #elif ENABLED(SPINDLE_SERVO) - MOVE_SERVO(SPINDLE_SERVO_NR, power); + servo[SPINDLE_SERVO_NR].move(power); #else WRITE(SPINDLE_LASER_ENA_PIN, enabled() ? SPINDLE_LASER_ACTIVE_STATE : !SPINDLE_LASER_ACTIVE_STATE); isReady = true; diff --git a/Marlin/src/gcode/config/M43.cpp b/Marlin/src/gcode/config/M43.cpp index de76b8e979..688b94c9bf 100644 --- a/Marlin/src/gcode/config/M43.cpp +++ b/Marlin/src/gcode/config/M43.cpp @@ -198,10 +198,10 @@ inline void servo_probe_test() { uint8_t i = 0; SERIAL_ECHOLNPGM(". Deploy & stow 4 times"); do { - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy safe_delay(500); deploy_state = READ(PROBE_TEST_PIN); - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow safe_delay(500); stow_state = READ(PROBE_TEST_PIN); } while (++i < 4); @@ -226,7 +226,7 @@ inline void servo_probe_test() { } // Ask the user for a trigger event and measure the pulse width. - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][0]); // Deploy safe_delay(500); SERIAL_ECHOLNPGM("** Please trigger probe within 30 sec **"); uint16_t probe_counter = 0; @@ -256,7 +256,7 @@ inline void servo_probe_test() { } else SERIAL_ECHOLNPGM("FAIL: Noise detected - please re-run test"); - MOVE_SERVO(probe_index, servo_angles[Z_PROBE_SERVO_NR][1]); // Stow + servo[probe_index].move(servo_angles[Z_PROBE_SERVO_NR][1]); // Stow return; } } diff --git a/Marlin/src/gcode/control/M280.cpp b/Marlin/src/gcode/control/M280.cpp index ed156e16e9..82981e44bc 100644 --- a/Marlin/src/gcode/control/M280.cpp +++ b/Marlin/src/gcode/control/M280.cpp @@ -56,14 +56,14 @@ void GcodeSuite::M280() { while (PENDING(now, end)) { safe_delay(50); now = _MIN(millis(), end); - MOVE_SERVO(servo_index, LROUND(aold + (anew - aold) * (float(now - start) / t))); + servo[servo_index].move(LROUND(aold + (anew - aold) * (float(now - start) / t))); } } #endif // POLARGRAPH - MOVE_SERVO(servo_index, anew); + servo[servo_index].move(anew); } else - DETACH_SERVO(servo_index); + servo[servo_index].detach(); } else SERIAL_ECHO_MSG(" Servo ", servo_index, ": ", servo[servo_index].read()); diff --git a/Marlin/src/gcode/control/M282.cpp b/Marlin/src/gcode/control/M282.cpp index e6f5ce7dcc..3ac5ac9f5b 100644 --- a/Marlin/src/gcode/control/M282.cpp +++ b/Marlin/src/gcode/control/M282.cpp @@ -36,7 +36,7 @@ void GcodeSuite::M282() { const int servo_index = parser.value_int(); if (WITHIN(servo_index, 0, NUM_SERVOS - 1)) - DETACH_SERVO(servo_index); + servo[servo_index].detach(); else SERIAL_ECHO_MSG("Servo ", servo_index, " out of range"); diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index 66e1c85028..2649aa41b6 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -343,7 +343,7 @@ FORCE_INLINE void probe_specific_action(const bool deploy) { #elif HAS_Z_SERVO_PROBE - MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]); + servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][deploy ? 0 : 1]); #elif EITHER(TOUCH_MI_PROBE, Z_PROBE_ALLEN_KEY) diff --git a/Marlin/src/module/servo.cpp b/Marlin/src/module/servo.cpp index 96d5ba9da8..2782be1f2b 100644 --- a/Marlin/src/module/servo.cpp +++ b/Marlin/src/module/servo.cpp @@ -39,19 +39,19 @@ hal_servo_t servo[NUM_SERVOS]; void servo_init() { #if NUM_SERVOS >= 1 && HAS_SERVO_0 servo[0].attach(SERVO0_PIN); - DETACH_SERVO(0); // Just set up the pin. We don't have a position yet. Don't move to a random position. + servo[0].detach(); // Just set up the pin. We don't have a position yet. Don't move to a random position. #endif #if NUM_SERVOS >= 2 && HAS_SERVO_1 servo[1].attach(SERVO1_PIN); - DETACH_SERVO(1); + servo[1].detach(); #endif #if NUM_SERVOS >= 3 && HAS_SERVO_2 servo[2].attach(SERVO2_PIN); - DETACH_SERVO(2); + servo[2].detach(); #endif #if NUM_SERVOS >= 4 && HAS_SERVO_3 servo[3].attach(SERVO3_PIN); - DETACH_SERVO(3); + servo[3].detach(); #endif } diff --git a/Marlin/src/module/servo.h b/Marlin/src/module/servo.h index cd55a317a2..2ed992aa03 100644 --- a/Marlin/src/module/servo.h +++ b/Marlin/src/module/servo.h @@ -103,14 +103,11 @@ }; #if HAS_Z_SERVO_PROBE - #define DEPLOY_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][0]) - #define STOW_Z_SERVO() MOVE_SERVO(Z_PROBE_SERVO_NR, servo_angles[Z_PROBE_SERVO_NR][1]) + #define DEPLOY_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][0]) + #define STOW_Z_SERVO() servo[Z_PROBE_SERVO_NR].move(servo_angles[Z_PROBE_SERVO_NR][1]) #endif #endif // HAS_SERVO_ANGLES -#define MOVE_SERVO(I, P) servo[I].move(P) -#define DETACH_SERVO(I) servo[I].detach() - extern hal_servo_t servo[NUM_SERVOS]; void servo_init(); diff --git a/Marlin/src/module/temperature.cpp b/Marlin/src/module/temperature.cpp index 531c52c09d..17ab08d4c2 100644 --- a/Marlin/src/module/temperature.cpp +++ b/Marlin/src/module/temperature.cpp @@ -1881,7 +1881,7 @@ void Temperature::manage_heater() { #endif #if ENABLED(CHAMBER_VENT) flag_chamber_excess_heat = false; - MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 90); + servo[CHAMBER_VENT_SERVO_NR].move(90); #endif } #endif @@ -1897,7 +1897,7 @@ void Temperature::manage_heater() { if (flag_chamber_excess_heat) { temp_chamber.soft_pwm_amount = 0; #if ENABLED(CHAMBER_VENT) - if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, temp_chamber.celsius <= temp_chamber.target ? 0 : 90); + if (!flag_chamber_off) servo[CHAMBER_VENT_SERVO_NR].move(temp_chamber.celsius <= temp_chamber.target ? 0 : 90); #endif } else { @@ -1910,7 +1910,7 @@ void Temperature::manage_heater() { temp_chamber.soft_pwm_amount = temp_chamber.celsius < temp_chamber.target ? (MAX_CHAMBER_POWER) >> 1 : 0; #endif #if ENABLED(CHAMBER_VENT) - if (!flag_chamber_off) MOVE_SERVO(CHAMBER_VENT_SERVO_NR, 0); + if (!flag_chamber_off) servo[CHAMBER_VENT_SERVO_NR].move(0); #endif } } diff --git a/Marlin/src/module/tool_change.cpp b/Marlin/src/module/tool_change.cpp index e2f1443048..4b292c92f9 100644 --- a/Marlin/src/module/tool_change.cpp +++ b/Marlin/src/module/tool_change.cpp @@ -116,7 +116,7 @@ void move_extruder_servo(const uint8_t e) { planner.synchronize(); if ((EXTRUDERS & 1) && e < EXTRUDERS - 1) { - MOVE_SERVO(_SERVO_NR(e), servo_angles[_SERVO_NR(e)][e & 1]); + servo[_SERVO_NR(e)].move(servo_angles[_SERVO_NR(e)][e & 1]); safe_delay(500); } } @@ -131,7 +131,7 @@ constexpr int8_t sns_index[2] = { SWITCHING_NOZZLE_SERVO_NR, SWITCHING_NOZZLE_E1_SERVO_NR }; constexpr int16_t sns_angles[2] = SWITCHING_NOZZLE_SERVO_ANGLES; planner.synchronize(); - MOVE_SERVO(sns_index[e], sns_angles[angle_index]); + servo[sns_index[e]].move(sns_angles[angle_index]); safe_delay(500); } @@ -142,7 +142,7 @@ void move_nozzle_servo(const uint8_t angle_index) { planner.synchronize(); - MOVE_SERVO(SWITCHING_NOZZLE_SERVO_NR, servo_angles[SWITCHING_NOZZLE_SERVO_NR][angle_index]); + servo[SWITCHING_NOZZLE_SERVO_NR].move(servo_angles[SWITCHING_NOZZLE_SERVO_NR][angle_index]); safe_delay(500); } @@ -443,7 +443,7 @@ void fast_line_to_current(const AxisEnum fr_axis) { _line_to_current(fr_axis, 0. inline void switching_toolhead_lock(const bool locked) { #ifdef SWITCHING_TOOLHEAD_SERVO_ANGLES const uint16_t swt_angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES; - MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, swt_angles[locked ? 0 : 1]); + servo[SWITCHING_TOOLHEAD_SERVO_NR].move(swt_angles[locked ? 0 : 1]); #elif PIN_EXISTS(SWT_SOLENOID) OUT_WRITE(SWT_SOLENOID_PIN, locked); gcode.dwell(10); From 0ca33429b679674f582532f8aa8c54ec60eaa655 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 9 Jun 2022 00:51:47 -0500 Subject: [PATCH 12/19] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20Misc.?= =?UTF-8?q?=20servo=20code=20cleanup?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/HAL/AVR/Servo.cpp | 2 +- Marlin/src/HAL/DUE/Servo.cpp | 21 ++++++++------------- Marlin/src/HAL/SAMD51/Servo.cpp | 4 ++-- Marlin/src/HAL/STM32F1/Servo.cpp | 16 ++++++++-------- Marlin/src/HAL/shared/servo.cpp | 13 +++++++------ Marlin/src/HAL/shared/servo_private.h | 12 ++++++------ 6 files changed, 32 insertions(+), 36 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 526352b773..6dc1e2fb07 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -66,7 +66,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s /************ static functions common to all instances ***********************/ -static inline void handle_interrupts(timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { +static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) { if (Channel[timer] < 0) *TCNTn = 0; // channel set to -1 indicated that refresh interval completed so reset the timer else { diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 5524aa9cef..72a7d649ef 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -52,7 +52,7 @@ static volatile int8_t Channel[_Nbr_16timers]; // counter for the s // ------------------------ /// Interrupt handler for the TC0 channel 1. // ------------------------ -void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); +void Servo_Handler(const timer16_Sequence_t, Tc*, const uint8_t); #ifdef _useTimer1 void HANDLER_FOR_TIMER1() { Servo_Handler(_timer1, TC_FOR_TIMER1, CHANNEL_FOR_TIMER1); } @@ -70,7 +70,7 @@ void Servo_Handler(timer16_Sequence_t timer, Tc *pTc, uint8_t channel); void HANDLER_FOR_TIMER5() { Servo_Handler(_timer5, TC_FOR_TIMER5, CHANNEL_FOR_TIMER5); } #endif -void Servo_Handler(timer16_Sequence_t timer, Tc *tc, uint8_t channel) { +void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) { // clear interrupt tc->TC_CHANNEL[channel].TC_SR; if (Channel[timer] < 0) @@ -113,26 +113,21 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_Start(tc, channel); } -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { #ifdef _useTimer1 - if (timer == _timer1) - _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); + if (timer == _timer1) _initISR(TC_FOR_TIMER1, CHANNEL_FOR_TIMER1, ID_TC_FOR_TIMER1, IRQn_FOR_TIMER1); #endif #ifdef _useTimer2 - if (timer == _timer2) - _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); + if (timer == _timer2) _initISR(TC_FOR_TIMER2, CHANNEL_FOR_TIMER2, ID_TC_FOR_TIMER2, IRQn_FOR_TIMER2); #endif #ifdef _useTimer3 - if (timer == _timer3) - _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); + if (timer == _timer3) _initISR(TC_FOR_TIMER3, CHANNEL_FOR_TIMER3, ID_TC_FOR_TIMER3, IRQn_FOR_TIMER3); #endif #ifdef _useTimer4 - if (timer == _timer4) - _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); + if (timer == _timer4) _initISR(TC_FOR_TIMER4, CHANNEL_FOR_TIMER4, ID_TC_FOR_TIMER4, IRQn_FOR_TIMER4); #endif #ifdef _useTimer5 - if (timer == _timer5) - _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); + if (timer == _timer5) _initISR(TC_FOR_TIMER5, CHANNEL_FOR_TIMER5, ID_TC_FOR_TIMER5, IRQn_FOR_TIMER5); #endif } diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 23ab21c615..04461653f9 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -124,7 +124,7 @@ HAL_SERVO_TIMER_ISR() { } } -void initISR(timer16_Sequence_t timer) { +void initISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); @@ -201,7 +201,7 @@ void initISR(timer16_Sequence_t timer) { } } -void finISR(timer16_Sequence_t timer) { +void finISR(const timer16_Sequence_t timer) { Tc * const tc = timer_config[SERVO_TC].pTc; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); diff --git a/Marlin/src/HAL/STM32F1/Servo.cpp b/Marlin/src/HAL/STM32F1/Servo.cpp index 8dc1ef7b6a..47ffb631cf 100644 --- a/Marlin/src/HAL/STM32F1/Servo.cpp +++ b/Marlin/src/HAL/STM32F1/Servo.cpp @@ -147,17 +147,17 @@ void libServo::move(const int32_t value) { uint16_t SR = timer_get_status(tdev); if (SR & TIMER_SR_CC1IF) { // channel 1 off #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 1); // off + OUT_WRITE_OD(SERVO0_PIN, HIGH); // off #else - OUT_WRITE(SERVO0_PIN, 0); + OUT_WRITE(SERVO0_PIN, LOW); #endif timer_reset_status_bit(tdev, TIMER_SR_CC1IF_BIT); } if (SR & TIMER_SR_CC2IF) { // channel 2 resume #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(SERVO0_PIN, 0); // on + OUT_WRITE_OD(SERVO0_PIN, LOW); // on #else - OUT_WRITE(SERVO0_PIN, 1); + OUT_WRITE(SERVO0_PIN, HIGH); #endif timer_reset_status_bit(tdev, TIMER_SR_CC2IF_BIT); } @@ -167,9 +167,9 @@ void libServo::move(const int32_t value) { timer_dev *tdev = HAL_get_timer_dev(MF_TIMER_SERVO0); if (!tdev) return false; #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(inPin, 1); + OUT_WRITE_OD(inPin, HIGH); #else - OUT_WRITE(inPin, 0); + OUT_WRITE(inPin, LOW); #endif timer_pause(tdev); @@ -200,9 +200,9 @@ void libServo::move(const int32_t value) { timer_disable_irq(tdev, 1); timer_disable_irq(tdev, 2); #ifdef SERVO0_PWM_OD - OUT_WRITE_OD(pin, 1); // off + OUT_WRITE_OD(pin, HIGH); // off #else - OUT_WRITE(pin, 0); + OUT_WRITE(pin, LOW); #endif } } diff --git a/Marlin/src/HAL/shared/servo.cpp b/Marlin/src/HAL/shared/servo.cpp index cfec6f3017..b838800de6 100644 --- a/Marlin/src/HAL/shared/servo.cpp +++ b/Marlin/src/HAL/shared/servo.cpp @@ -65,7 +65,7 @@ uint8_t ServoCount = 0; // the total number of attached /************ static functions common to all instances ***********************/ -static boolean isTimerActive(timer16_Sequence_t timer) { +static bool anyTimerChannelActive(const timer16_Sequence_t timer) { // returns true if any servo is active on this timer LOOP_L_N(channel, SERVOS_PER_TIMER) { if (SERVO(timer, channel).Pin.isActive) @@ -101,17 +101,18 @@ int8_t Servo::attach(const int inPin, const int inMin, const int inMax) { max = (MAX_PULSE_WIDTH - inMax) / 4; // initialize the timer if it has not already been initialized - timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) initISR(timer); - servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for isTimerActive + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (!anyTimerChannelActive(timer)) initISR(timer); + servo_info[servoIndex].Pin.isActive = true; // this must be set after the check for anyTimerChannelActive return servoIndex; } void Servo::detach() { servo_info[servoIndex].Pin.isActive = false; - timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); - if (!isTimerActive(timer)) finISR(timer); + const timer16_Sequence_t timer = SERVO_INDEX_TO_TIMER(servoIndex); + if (!anyTimerChannelActive(timer)) finISR(timer); + //pinMode(servo_info[servoIndex].Pin.nbr, INPUT); // set servo pin to input } void Servo::write(int value) { diff --git a/Marlin/src/HAL/shared/servo_private.h b/Marlin/src/HAL/shared/servo_private.h index d85d8da8ba..021e0cb81d 100644 --- a/Marlin/src/HAL/shared/servo_private.h +++ b/Marlin/src/HAL/shared/servo_private.h @@ -70,10 +70,10 @@ #define ticksToUs(_ticks) (unsigned(_ticks) * (SERVO_TIMER_PRESCALER) / clockCyclesPerMicrosecond()) // convenience macros -#define SERVO_INDEX_TO_TIMER(_servo_nbr) ((timer16_Sequence_t)(_servo_nbr / (SERVOS_PER_TIMER))) // returns the timer controlling this servo -#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // returns the index of the servo on this timer -#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // macro to access servo index by timer and channel -#define SERVO(_timer,_channel) (servo_info[SERVO_INDEX(_timer,_channel)]) // macro to access servo class by timer and channel +#define SERVO_INDEX_TO_TIMER(_servo_nbr) timer16_Sequence_t(_servo_nbr / (SERVOS_PER_TIMER)) // the timer controlling this servo +#define SERVO_INDEX_TO_CHANNEL(_servo_nbr) (_servo_nbr % (SERVOS_PER_TIMER)) // the index of the servo on this timer +#define SERVO_INDEX(_timer,_channel) ((_timer*(SERVOS_PER_TIMER)) + _channel) // servo index by timer and channel +#define SERVO(_timer,_channel) servo_info[SERVO_INDEX(_timer,_channel)] // servo class by timer and channel // Types @@ -94,5 +94,5 @@ extern ServoInfo_t servo_info[MAX_SERVOS]; // Public functions -extern void initISR(timer16_Sequence_t timer); -extern void finISR(timer16_Sequence_t timer); +extern void initISR(const timer16_Sequence_t timer); +extern void finISR(const timer16_Sequence_t timer); From 8af96b8cdde75b4d7be2c1c55a870e62fbf3f481 Mon Sep 17 00:00:00 2001 From: InsanityAutomation Date: Thu, 9 Jun 2022 15:48:08 -0400 Subject: [PATCH 13/19] Update Servo.cpp --- Marlin/src/HAL/DUE/Servo.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index fd6ae1d79a..0f179fbe16 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -86,7 +86,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 4, // at least REFRESH_INTERVAL has elapsed + const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + (256 / SERVO_TIMER_PRESCALER), // at least REFRESH_INTERVAL has elapsed ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed tc->TC_CHANNEL[channel].TC_RA = max(cval, ival); From 5fbb7cbe6ad28bc7c22f8a3d6a2501a58e827a9a Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 19:05:10 -0500 Subject: [PATCH 14/19] ws --- Marlin/src/HAL/DUE/Servo.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 0f179fbe16..9800a192ba 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -86,7 +86,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + (256 / SERVO_TIMER_PRESCALER), // at least REFRESH_INTERVAL has elapsed + const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + (256 / SERVO_TIMER_PRESCALER), // at least REFRESH_INTERVAL has elapsed ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed tc->TC_CHANNEL[channel].TC_RA = max(cval, ival); @@ -101,7 +101,7 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_CMR_WAVE | // Waveform mode TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC - /* 84MHz, MCK/32, for 1.5ms: 3937 */ + // 84MHz, MCK/32, for 1.5ms: 3937 TC_SetRA(tc, channel, 2625); // 1ms /* Configure and enable interrupt */ From 9bb87da816e4d55f591080354e052714054eca15 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 19:52:03 -0500 Subject: [PATCH 15/19] apply SERVO_TIMER_PRESCALER to intervals --- Marlin/src/HAL/AVR/Servo.cpp | 6 +++--- Marlin/src/HAL/DUE/Servo.cpp | 13 ++++++------- Marlin/src/HAL/SAMD51/Servo.cpp | 5 +++-- 3 files changed, 12 insertions(+), 12 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index b3f65a36b2..23335bc992 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -81,9 +81,9 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int tval = ((unsigned)*TCNTn) + 4, // at least REFRESH_INTERVAL has elapsed - ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed - *OCRnA = max(tval, ival); + const unsigned int cval = ((unsigned)*TCNTn) + 32 / (SERVO_TIMER_PRESCALER), // allow 32 cycles to ensure the next OCR1A not missed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed + *OCRnA = max(cval, ival); Channel[timer] = -1; // reset the timer counter to 0 on the next call } diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 9800a192ba..851ce5743c 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -86,8 +86,8 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel } else { // finished all channels so wait for the refresh period to expire before starting over - const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + (256 / SERVO_TIMER_PRESCALER), // at least REFRESH_INTERVAL has elapsed - ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // allow a few ticks to ensure the next OCR1A not missed + const unsigned int cval = tc->TC_CHANNEL[channel].TC_CV + 128 / (SERVO_TIMER_PRESCALER), // allow 128 cycles to ensure the next CV not missed + ival = (unsigned int)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed tc->TC_CHANNEL[channel].TC_RA = max(cval, ival); Channel[timer] = -1; // reset the timer CCR on the next call @@ -101,13 +101,12 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_CMR_WAVE | // Waveform mode TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC - // 84MHz, MCK/32, for 1.5ms: 3937 - TC_SetRA(tc, channel, 2625); // 1ms + // Wait 1ms before the first ISR + TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms - /* Configure and enable interrupt */ + // Configure and enable interrupt NVIC_EnableIRQ(irqn); - // TC_IER_CPAS: RA Compare - tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; + tc->TC_CHANNEL[channel].TC_IER = TC_IER_CPAS; // TC_IER_CPAS: RA Compare // Enables the timer clock and performs a software reset to start the counting TC_Start(tc, channel); diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index dfe188fde3..34973c0e18 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -108,10 +108,11 @@ HAL_SERVO_TIMER_ISR() { const uint16_t tcCounterValue = getTimerCount(), ival = (uint16_t)usToTicks(REFRESH_INTERVAL); - if ((TC_COUNTER_START_VAL - tcCounterValue) + 4UL < ival) // allow a few ticks to ensure the next OCR1A not missed + constexpr uint16_t minticks = 256 / (SERVO_TIMER_PRESCALER); + if ((TC_COUNTER_START_VAL - tcCounterValue) + minticks < ival) // allow 256 cycles to ensure the next OCR1A not missed tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - ival; else - tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL); // at least REFRESH_INTERVAL has elapsed + tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - minticks); // at least REFRESH_INTERVAL has elapsed } if (tcChannel == 0) { SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); From a03ee01ceeb78d0b3566f4c56bd88b3857c902fe Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 20:09:05 -0500 Subject: [PATCH 16/19] match samd51 to others --- Marlin/src/HAL/AVR/Servo.cpp | 4 ++-- Marlin/src/HAL/DUE/Servo.cpp | 2 +- Marlin/src/HAL/SAMD51/Servo.cpp | 39 +++++++++++++-------------------- 3 files changed, 18 insertions(+), 27 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 23335bc992..7b48c0ca19 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -76,8 +76,8 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui Channel[timer] = ++cho; // Handle the next channel (or 0) if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { *OCRnA = *TCNTn + SERVO(timer, cho).ticks; // set compare to current ticks plus duration - if (SERVO(timer, cho).Pin.isActive) // check if activated - extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // if active, set HIGH. (set LOW next interrupt) + if (SERVO(timer, cho).Pin.isActive) // activated? + extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH } else { // finished all channels so wait for the refresh period to expire before starting over diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 851ce5743c..9d67859b51 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -79,7 +79,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW Channel[timer] = ++cho; // go to the next channel (or 0) - if (SERVO_INDEX(timer, cho) < ServoCount && cho < SERVOS_PER_TIMER) { + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { tc->TC_CHANNEL[channel].TC_RA = tc->TC_CHANNEL[channel].TC_CV + SERVO(timer, cho).ticks; if (SERVO(timer, cho).Pin.isActive) // activated? extDigitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index 34973c0e18..c7a89806f6 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -77,8 +77,8 @@ HAL_SERVO_TIMER_ISR() { ; const uint8_t tcChannel = TIMER_TCCHANNEL(timer); - int8_t cho = currentServoIndex[timer]; - if (cho < 0) { + int8_t cho = currentServoIndex[timer]; // Handle the prior servo first + if (cho < 0) { // Servo -1 indicates the refresh interval completed... #if defined(_useTimer1) && defined(_useTimer2) if (currentServoIndex[timer ^ 1] >= 0) { // Wait for both channels @@ -87,42 +87,33 @@ HAL_SERVO_TIMER_ISR() { return; } #endif - tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; + tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT); } - else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) - digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse this channel low if activated + else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel... handled and activated? + digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW - // Select the next servo controlled by this timer - currentServoIndex[timer] = ++cho; - - if (SERVO_INDEX(timer, cho) < ServoCount && cho < SERVOS_PER_TIMER) { - if (SERVO(timer, cho).Pin.isActive) // check if activated - digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // it's an active channel so pulse it high + currentServoIndex[timer] = ++cho; // go to the next channel (or 0) + if (cho < SERVOS_PER_TIMER && SERVO_INDEX(timer, cho) < ServoCount) { + if (SERVO(timer, cho).Pin.isActive) // activated? + digitalWrite(SERVO(timer, cho).Pin.nbr, HIGH); // yes: pulse HIGH tc->COUNT16.CC[tcChannel].reg = getTimerCount() - (uint16_t)SERVO(timer, cho).ticks; } else { // finished all channels so wait for the refresh period to expire before starting over - currentServoIndex[timer] = -1; // this will get incremented at the end of the refresh period to start again at the first channel - - const uint16_t tcCounterValue = getTimerCount(), ival = (uint16_t)usToTicks(REFRESH_INTERVAL); - - constexpr uint16_t minticks = 256 / (SERVO_TIMER_PRESCALER); - if ((TC_COUNTER_START_VAL - tcCounterValue) + minticks < ival) // allow 256 cycles to ensure the next OCR1A not missed - tc->COUNT16.CC[tcChannel].reg = TC_COUNTER_START_VAL - ival; - else - tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - minticks); // at least REFRESH_INTERVAL has elapsed + currentServoIndex[timer] = -1; // reset the timer COUNT.reg on the next call + const uint16_t cval = getTimerCount() - 256 / (SERVO_TIMER_PRESCALER), // allow 256 cycles to ensure the next CV not missed + ival = (TC_COUNTER_START_VAL) - (uint16_t)usToTicks(REFRESH_INTERVAL); // at least REFRESH_INTERVAL has elapsed + tc->COUNT16.CC[tcChannel].reg = min(cval, ival); } if (tcChannel == 0) { SYNC(tc->COUNT16.SYNCBUSY.bit.CC0); - // Clear the interrupt - tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; + tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC0; // Clear the interrupt } else { SYNC(tc->COUNT16.SYNCBUSY.bit.CC1); - // Clear the interrupt - tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; + tc->COUNT16.INTFLAG.reg = TC_INTFLAG_MC1; // Clear the interrupt } } From b3f2f441240832d6d5f9232dbc8e78b9be0a0f9a Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 20:11:49 -0500 Subject: [PATCH 17/19] set inactive servos LOW --- Marlin/src/HAL/AVR/Servo.cpp | 2 +- Marlin/src/HAL/DUE/Servo.cpp | 2 +- Marlin/src/HAL/SAMD51/Servo.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp index 7b48c0ca19..1b4208e365 100644 --- a/Marlin/src/HAL/AVR/Servo.cpp +++ b/Marlin/src/HAL/AVR/Servo.cpp @@ -70,7 +70,7 @@ static inline void handle_interrupts(const timer16_Sequence_t timer, volatile ui int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first if (cho < 0) // Channel -1 indicates the refresh interval completed... *TCNTn = 0; // ...so reset the timer - else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel ... handled and active? + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW Channel[timer] = ++cho; // Handle the next channel (or 0) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 9d67859b51..e2d1518ebc 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -75,7 +75,7 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel int8_t cho = Channel[timer]; // Handle the prior Channel[timer] first if (cho < 0) // Channel -1 indicates the refresh interval completed... tc->TC_CHANNEL[channel].TC_CCR |= TC_CCR_SWTRG; // ...so reset the timer - else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel... handled and active? + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW Channel[timer] = ++cho; // go to the next channel (or 0) diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp index c7a89806f6..3fed7cf93e 100644 --- a/Marlin/src/HAL/SAMD51/Servo.cpp +++ b/Marlin/src/HAL/SAMD51/Servo.cpp @@ -90,7 +90,7 @@ HAL_SERVO_TIMER_ISR() { tc->COUNT16.COUNT.reg = TC_COUNTER_START_VAL; // ...so reset the timer SYNC(tc->COUNT16.SYNCBUSY.bit.COUNT); } - else if (SERVO_INDEX(timer, cho) < ServoCount && SERVO(timer, cho).Pin.isActive) // prior channel... handled and activated? + else if (SERVO_INDEX(timer, cho) < ServoCount) // prior channel handled? digitalWrite(SERVO(timer, cho).Pin.nbr, LOW); // pulse the prior channel LOW currentServoIndex[timer] = ++cho; // go to the next channel (or 0) From 701ab0db8e02a5bf9abd194051415eba28fd32f7 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 21:09:25 -0500 Subject: [PATCH 18/19] define HAL_TIMER_PRESCALER --- Marlin/src/HAL/DUE/Servo.cpp | 8 +++++--- Marlin/src/HAL/DUE/timers.cpp | 9 +++++++-- Marlin/src/HAL/DUE/timers.h | 3 ++- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index e2d1518ebc..565bd5a3cc 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -97,9 +97,11 @@ void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { pmc_enable_periph_clk(id); TC_Configure(tc, channel, - TC_CMR_TCCLKS_TIMER_CLOCK1 | // MCK/2 - TC_CMR_WAVE | // Waveform mode - TC_CMR_WAVSEL_UP_RC ); // Counter running up and reset when equals to RC + TC_CMR_WAVE // Waveform mode + | TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC + | (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2 + | (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32 + ); // Wait 1ms before the first ISR TC_SetRA(tc, channel, (F_CPU) / (SERVO_TIMER_PRESCALER) / 1000UL); // 1ms diff --git a/Marlin/src/HAL/DUE/timers.cpp b/Marlin/src/HAL/DUE/timers.cpp index a7bf7fbd6d..eb075be32e 100644 --- a/Marlin/src/HAL/DUE/timers.cpp +++ b/Marlin/src/HAL/DUE/timers.cpp @@ -89,10 +89,15 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { NVIC_SetPriority(irq, timer_config[timer_num].priority); // wave mode, reset counter on match with RC, - TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC | TC_CMR_TCCLKS_TIMER_CLOCK1); + TC_Configure(tc, channel, + TC_CMR_WAVE + | TC_CMR_WAVSEL_UP_RC + | (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) + | (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) + ); // Set compare value - TC_SetRC(tc, channel, VARIANT_MCK / 2 / frequency); + TC_SetRC(tc, channel, VARIANT_MCK / (HAL_TIMER_PRESCALER) / frequency); // And start timer TC_Start(tc, channel); diff --git a/Marlin/src/HAL/DUE/timers.h b/Marlin/src/HAL/DUE/timers.h index bcfd07e268..dc35c77e63 100644 --- a/Marlin/src/HAL/DUE/timers.h +++ b/Marlin/src/HAL/DUE/timers.h @@ -35,7 +35,8 @@ typedef uint32_t hal_timer_t; #define HAL_TIMER_TYPE_MAX 0xFFFFFFFF -#define HAL_TIMER_RATE ((F_CPU) / 2) // frequency of timers peripherals +#define HAL_TIMER_PRESCALER 2 +#define HAL_TIMER_RATE ((F_CPU) / (HAL_TIMER_PRESCALER)) // frequency of timers peripherals #ifndef MF_TIMER_STEP #define MF_TIMER_STEP 2 // Timer Index for Stepper From ad4cf3e332f046ee0f1a91d4df90782dbc9bfd15 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 10 Jun 2022 22:22:40 -0500 Subject: [PATCH 19/19] pick best clock for prescaler --- Marlin/src/HAL/DUE/Servo.cpp | 6 ++++-- Marlin/src/HAL/DUE/timers.cpp | 6 ++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/Marlin/src/HAL/DUE/Servo.cpp b/Marlin/src/HAL/DUE/Servo.cpp index 565bd5a3cc..f693eb1edf 100644 --- a/Marlin/src/HAL/DUE/Servo.cpp +++ b/Marlin/src/HAL/DUE/Servo.cpp @@ -99,8 +99,10 @@ static void _initISR(Tc *tc, uint32_t channel, uint32_t id, IRQn_Type irqn) { TC_Configure(tc, channel, TC_CMR_WAVE // Waveform mode | TC_CMR_WAVSEL_UP_RC // Counter running up and reset when equal to RC - | (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2 - | (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32 + | (SERVO_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) // MCK/2 + | (SERVO_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) // MCK/8 + | (SERVO_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) // MCK/32 + | (SERVO_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) // MCK/128 ); // Wait 1ms before the first ISR diff --git a/Marlin/src/HAL/DUE/timers.cpp b/Marlin/src/HAL/DUE/timers.cpp index eb075be32e..e5647817b6 100644 --- a/Marlin/src/HAL/DUE/timers.cpp +++ b/Marlin/src/HAL/DUE/timers.cpp @@ -92,8 +92,10 @@ void HAL_timer_start(const uint8_t timer_num, const uint32_t frequency) { TC_Configure(tc, channel, TC_CMR_WAVE | TC_CMR_WAVSEL_UP_RC - | (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) - | (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) + | (HAL_TIMER_PRESCALER == 2 ? TC_CMR_TCCLKS_TIMER_CLOCK1 : 0) + | (HAL_TIMER_PRESCALER == 8 ? TC_CMR_TCCLKS_TIMER_CLOCK2 : 0) + | (HAL_TIMER_PRESCALER == 32 ? TC_CMR_TCCLKS_TIMER_CLOCK3 : 0) + | (HAL_TIMER_PRESCALER == 128 ? TC_CMR_TCCLKS_TIMER_CLOCK4 : 0) ); // Set compare value