From 5932b4d1014a978916719b43f59ffb026b307bb9 Mon Sep 17 00:00:00 2001
From: Neil McKechnie <neilmck999@gmail.com>
Date: Thu, 12 Aug 2021 12:02:18 +0100
Subject: [PATCH] Remove unnecessary servo output demands.

If start and end position of a servo movement are identical, skip all but the last step.
---
 IO_PCA9685.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/IO_PCA9685.cpp b/IO_PCA9685.cpp
index 8ddf409..a6bde29 100644
--- a/IO_PCA9685.cpp
+++ b/IO_PCA9685.cpp
@@ -196,6 +196,10 @@ void PCA9685::updatePosition(uint8_t pin) {
   struct ServoData *s = _servoData[pin];
   if (!s) return;
   if (s->numSteps == 0) return; // No animation in progress
+  if (s->stepNumber == 0 && s->fromPosition == s->toPosition) {
+    // No movement required, so go straight to final step
+    s->stepNumber = s->numSteps;
+  }
   if (s->stepNumber < s->numSteps) {
     // Animation in progress, reposition servo
     s->stepNumber++;