From fb32be56d3c6b3bcef5a401aeaf8bfa2a3fcb642 Mon Sep 17 00:00:00 2001
From: Mark van Renswoude <mark@x2software.net>
Date: Mon, 7 Jan 2019 20:07:50 +0100
Subject: [PATCH] Fixed noise and unreliability issues, cleaned up code

---
 src/main.cpp | 137 ++++++++++++++++++++++++++++++++++-----------------
 1 file changed, 91 insertions(+), 46 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index cbb2707..f571011 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -34,8 +34,9 @@ static const uint16_t ServoMinPulse = 544;
 static const uint16_t ServoMaxPulse = 2400;
 
 
-static const uint16_t ServoPulseInterval = 20;
-static const uint16_t ServoStabilizeDelay = 100;
+static const uint16_t ServoPulseInterval = 20;      // How often servo pulses are sent (around 20 ms is the protocol)
+static const uint16_t ServoHoldDelay = 500;         // How long the position is held after moving, to account for the servo catching up
+static const uint16_t ServoStabilizeDelay = 200;    // How long to wait for the voltage to stabilize before checking calibration
 
 
 int offPosition;              // 0 - 180 degrees, read from "Off" potentiometer
@@ -44,16 +45,28 @@ int speed;                    // 0 - 1023, read from "Speed" potentiometer
 float degreesPerMilli;        // Degrees per millisecond
 
 
+enum State
+{
+  Idle,
+  Holding,
+  Stabilizing,
+  Moving,
+  Calibrating,
+  ZzZzZz
+};
+
+
+State state;                  // The current state
 bool lastOn;                  // If the switch was on when it was last flipped
 int startPosition;            // The position the servo was in when the switch was flipped
 unsigned long startTime;      // The last time the switch was flipped
-unsigned long idleTime;       // The last time anything happened
+unsigned long lastChangeTime; // The last time anything happened
 
-bool servoEnabled;            // Whether or not pulses should be sent to the servo
 int servoPosition;            // The desired position of the servo
 unsigned long servoPulseTime; // The last time the servo pulse was sent
 
 
+
 #ifdef DEBUG
 SoftwareSerial debug(-1,3);
 #endif
@@ -68,6 +81,13 @@ void sleepUntilSwitch();
 float floatMap(float x, float in_min, float in_max, float out_min, float out_max);
 
 
+// Macros
+#define setState(newState) { state = newState; lastChangeTime = currentTime; }
+#define isServoEnabled() (state == Holding || state == Moving || state == Calibrating)
+#define timePassed() (currentTime - lastChangeTime)
+
+
+
 void setup()
 {
   #ifdef DEBUG
@@ -86,12 +106,11 @@ void setup()
 
   updateSettings();
 
-  lastOn = false;
+  state = Holding;
   startPosition = offPosition;
   servoPosition = offPosition;
 
   servoPulseTime = 0;
-  servoEnabled = true;
 
   #ifdef DEBUG
   debug.println("Started");
@@ -103,7 +122,7 @@ void loop()
 {
   unsigned long currentTime = millis();
 
-  if (servoEnabled)
+  if (isServoEnabled())
     pulseServo(currentTime);
 
   bool isOn = (digitalRead(PinSwitch) == LOW);
@@ -118,54 +137,80 @@ void loop()
     lastOn = isOn;
     startPosition = servoPosition;
     startTime = currentTime;
-    idleTime = startTime;
-  }
 
-  int targetPosition = isOn ? onPosition : offPosition;
-  if (servoPosition != targetPosition)
-  {
-    servoPosition = getNewPosition(currentTime, targetPosition);
-    servoEnabled = true;
-    idleTime = currentTime;
-  }
-  else if (servoEnabled)
-  {
-    // Ensure we pulsed the last position
-    currentTime = millis();
-
-    delay(ServoPulseInterval);
-    pulseServo(currentTime);
-    delay(ServoPulseInterval);
-
-    // Stop sending pulses when we reach the destination, to prevent
-    // power consumption and possible buzzing
-    servoEnabled = false;
-    idleTime = currentTime;
+    setState(Moving);
   }
 
 
-  // Don't update the settings while the servo is moving, as the
-  // voltage drop can cause fluctuating results
-  if (!servoEnabled && currentTime - idleTime >= ServoStabilizeDelay)
+  switch (state)
   {
-    if (updateSettings())
+    case Idle:
     {
-      // Immediately go to the new position
-      currentTime = millis();
-      servoPosition = isOn ? onPosition : offPosition;
+      // Only check the settings first when the servo is idle, as the
+      // voltage drop can cause fluctuating results
+      if (updateSettings())
+      {
+        setState(Calibrating);
+        servoPosition = isOn ? onPosition : offPosition;
+      }
+      else if (timePassed() >= SleepTime)
+      {
+        state = ZzZzZz;
+        sleepUntilSwitch();
+      }
 
-      delay(ServoPulseInterval);
-      pulseServo(currentTime);
-      delay(ServoPulseInterval);
-
-      idleTime = currentTime;
+      break;
     }
-  }
 
-  // Stay awake for a bit to allow changes to the on and off positions
-  // to take effect immediately
-  if (currentTime - idleTime >= SleepTime)
-    sleepUntilSwitch();
+    case Holding:
+    {
+      // Keep the servo signal on until ServoHoldDelay has passed
+      if (timePassed() >= ServoHoldDelay)
+        setState(Stabilizing);
+
+      break;
+    }
+
+    case Stabilizing:
+    {
+      // Keep the servo signal off until ServoStabilizeDelay has passed
+      if (timePassed() >= ServoStabilizeDelay)
+        setState(Idle);
+
+      break;
+    }
+
+    case Moving:
+    {
+      int targetPosition = isOn ? onPosition : offPosition;
+      servoPosition = getNewPosition(currentTime, targetPosition);
+
+      if (servoPosition == targetPosition)
+        setState(Holding);
+
+      break;
+    }
+
+    case Calibrating:
+    {
+      // While calibrating, update immediatly, slight variations
+      // aren't that much of a concern at this point
+      if (updateSettings())
+      {
+        lastChangeTime = currentTime;
+        servoPosition = isOn ? onPosition : offPosition;
+      }
+      else if (timePassed() >= ServoHoldDelay)
+        setState(Idle);
+
+      break;
+    }
+
+    case ZzZzZz:
+      // Apparantly we were woken up without a switch change, go back to sleep
+      sleepUntilSwitch();
+      break;
+  }
 }