diff --git a/Marlin/src/HAL/AVR/Servo.cpp b/Marlin/src/HAL/AVR/Servo.cpp
index 526352b773..1b4208e365 100644
--- a/Marlin/src/HAL/AVR/Servo.cpp
+++ b/Marlin/src/HAL/AVR/Servo.cpp
@@ -66,27 +66,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) {
-  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
-  }
+static inline void handle_interrupts(const timer16_Sequence_t timer, volatile uint16_t* TCNTn, volatile uint16_t* OCRnA) {
+  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)                      // prior channel handled?
+    extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW);                  // pulse the prior channel LOW
 
-  Channel[timer]++;    // increment to the next channel
-  if (SERVO_INDEX(timer, Channel[timer]) < ServoCount && 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)                               // 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
-    if (((unsigned)*TCNTn) + 4 < usToTicks(REFRESH_INTERVAL))    // allow a few ticks to ensure the next OCR1A not missed
-      *OCRnA = (unsigned int)usToTicks(REFRESH_INTERVAL);
-    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
+    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
   }
 }
 
@@ -123,7 +122,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 +181,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 +191,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 +202,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 2d33808b4c..f693eb1edf 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,75 +70,67 @@ 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) {
-  // clear interrupt
-  tc->TC_CHANNEL[channel].TC_SR;
+void Servo_Handler(const timer16_Sequence_t timer, Tc *tc, const uint8_t channel) {
+  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)                      // prior channel handled?
+    extDigitalWrite(SERVO(timer, cho).Pin.nbr, LOW);                  // pulse the prior channel LOW
 
-  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)
-    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) {
-    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
+  Channel[timer] = ++cho;                                             // go to the next channel (or 0)
+  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
   }
   else {
     // finished all channels so wait for the refresh period to expire before starting over
-    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
-  }
+    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
+  }
 }
 
 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_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 ==   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
+  );
 
-  /* 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);
 }
 
-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/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/DUE/timers.cpp b/Marlin/src/HAL/DUE/timers.cpp
index a7bf7fbd6d..e5647817b6 100644
--- a/Marlin/src/HAL/DUE/timers.cpp
+++ b/Marlin/src/HAL/DUE/timers.cpp
@@ -89,10 +89,17 @@ 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 ==   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
-  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
diff --git a/Marlin/src/HAL/SAMD51/Servo.cpp b/Marlin/src/HAL/SAMD51/Servo.cpp
index 23ab21c615..3fed7cf93e 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];                // 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
@@ -86,45 +87,37 @@ 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, 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)        // prior channel handled?
+    digitalWrite(SERVO(timer, cho).Pin.nbr, LOW);       // pulse the prior channel LOW
 
-  // Select the next servo controlled by this timer
-  currentServoIndex[timer]++;
+  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
 
-  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
-
-    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();
-
-    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);
-    else
-      tc->COUNT16.CC[tcChannel].reg = (uint16_t)(tcCounterValue - 4UL);               // 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
   }
 }
 
-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 +194,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);
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 8dfdd1ecd9..a816b1a456 100644
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@@ -346,7 +346,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 bdef1bc2c2..dbd95121dc 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(SWITCHING_NOZZLE_SERVO_DWELL);
     }
 
@@ -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(SWITCHING_NOZZLE_SERVO_DWELL);
     }
 
@@ -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);