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
   }
 }