From 1fe3ec02a34e9a0ccf6509cdb610eb64ba482990 Mon Sep 17 00:00:00 2001
From: zzzz <alessioklesen@gmail.com>
Date: Tue, 30 Apr 2024 00:56:50 +0200
Subject: [PATCH] Replaced servo with continuous servo and implemented utils
 for simple control

---
 src/ContinuousServo.cpp | 40 ++++++++++++++++++++++++++++++++++++++++
 src/ContinuousServo.hpp | 18 ++++++++++++++++++
 src/TeaTimer.cpp        |  9 +++++----
 src/TeaTimer.hpp        | 10 ++++------
 src/main.cpp            | 23 +++++++++++------------
 5 files changed, 78 insertions(+), 22 deletions(-)
 create mode 100644 src/ContinuousServo.cpp
 create mode 100644 src/ContinuousServo.hpp

diff --git a/src/ContinuousServo.cpp b/src/ContinuousServo.cpp
new file mode 100644
index 0000000..efacbdb
--- /dev/null
+++ b/src/ContinuousServo.cpp
@@ -0,0 +1,40 @@
+#include "ContinuousServo.hpp"
+
+// 95 -> neutral, 0 or 180 -> full speed
+
+ContinuousServo::ContinuousServo(int pin)
+{
+    myservo.attach(pin);
+    myservo.write(NEUTRAL_POS);
+}
+
+void ContinuousServo::moveServoForMillisClockwise(unsigned long duration)
+{
+    if (m_isTurning)
+    {
+        return;
+    }
+    myservo.write(NEUTRAL_POS + 50);
+    m_endTurningTime = millis() + duration;
+    m_isTurning = true;
+}
+void ContinuousServo::moveServoForMillisCounterClockwise(unsigned long duration)
+{
+    if (m_isTurning)
+    {
+        return;
+    }
+    myservo.write(NEUTRAL_POS - 50);
+    m_endTurningTime = millis() + duration;
+    m_isTurning = true;
+}
+
+void ContinuousServo::tick()
+{
+    if (millis() >= m_endTurningTime && m_isTurning)
+    {
+        // stop servo when time limit reached
+        myservo.write(NEUTRAL_POS);
+        m_isTurning = false;
+    }
+}
\ No newline at end of file
diff --git a/src/ContinuousServo.hpp b/src/ContinuousServo.hpp
new file mode 100644
index 0000000..2c361d6
--- /dev/null
+++ b/src/ContinuousServo.hpp
@@ -0,0 +1,18 @@
+#pragma once
+#include <Servo.h>
+
+class ContinuousServo
+{
+public:
+    ContinuousServo(int pin);
+    void moveServoForMillisClockwise(unsigned long duration);
+    void moveServoForMillisCounterClockwise(unsigned long duration);
+    void tick();
+    bool isTurning() { return m_isTurning; }
+
+private:
+    Servo myservo;
+    bool m_isTurning = false;
+    unsigned long m_endTurningTime;
+    const int NEUTRAL_POS = 95;
+};
diff --git a/src/TeaTimer.cpp b/src/TeaTimer.cpp
index f8b5f94..4660722 100644
--- a/src/TeaTimer.cpp
+++ b/src/TeaTimer.cpp
@@ -1,6 +1,6 @@
 #include "TeaTimer.hpp"
 
-void TeaTimer::init(SmartServo *t_servo)
+void TeaTimer::init(ContinuousServo *t_servo)
 {
     m_servo = t_servo;
 }
@@ -10,7 +10,7 @@ void TeaTimer::beginSteeping(unsigned long t_steepingDuration)
 {
     if (m_isSteeping)
         return;
-    m_servo->moveServoTo180();
+    m_servo->moveServoForMillisClockwise(2000);
     m_endSteepingTime = millis() + t_steepingDuration;
     m_isSteeping = true;
 }
@@ -18,14 +18,15 @@ void TeaTimer::beginSteeping(unsigned long t_steepingDuration)
 // Move servo up, stop timer immediately.
 void TeaTimer::end_steeping()
 {
-    m_servo->moveServoToZero();
+    m_servo->moveServoForMillisCounterClockwise(2000);
     m_isSteeping = false;
 }
 
 // Called at start of update.
 void TeaTimer::tick()
 {
-    if (millis() >= m_endSteepingTime)
+    m_servo->tick();
+    if (millis() >= m_endSteepingTime && m_isSteeping)
     {
         end_steeping();
     }
diff --git a/src/TeaTimer.hpp b/src/TeaTimer.hpp
index 84b1adf..931b59e 100644
--- a/src/TeaTimer.hpp
+++ b/src/TeaTimer.hpp
@@ -1,11 +1,12 @@
 #pragma once
 #include <Arduino.h>
 #include "SmartServo.hpp"
+#include "ContinuousServo.hpp"
 
 class TeaTimer
 {
 public:
-    void init(SmartServo *t_servo);
+    void init(ContinuousServo *t_servo);
     // Move servo down, start timer.
     void beginSteeping(unsigned long t_steepingDuration);
     // Move servo up, stop timer immediately.
@@ -13,14 +14,11 @@ public:
     // Called at start of update.
     void tick();
     unsigned long remainingSteepTimeSeconds();
-    bool isSteeping()
-    {
-        return m_isSteeping;
-    };
+    bool isSteeping() { return m_isSteeping; };
 
 private:
     bool m_isSteeping = false;
     // The time in millis when steeping is done.
     unsigned long m_endSteepingTime;
-    SmartServo *m_servo;
+    ContinuousServo *m_servo;
 };
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 082988e..aef0654 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,6 +6,7 @@
 
 #include "TeaTimer.hpp"
 #include "SmartServo.hpp"
+#include "ContinuousServo.hpp"
 
 #define WIRE Wire
 
@@ -15,14 +16,14 @@
 
 auto display = Adafruit_SSD1306(128, 32, &WIRE);
 
-SmartServo smartServo(SERVO_PIN);
+ContinuousServo contServo(SERVO_PIN);
 TeaTimer teaTimer;
 
 void setup()
 {
   // Serial.begin(9600);
 
-  teaTimer.init(&smartServo);
+  teaTimer.init(&contServo);
 
   pinMode(LED_BUILTIN, OUTPUT);
   pinMode(TOUCH_BTN_PIN, INPUT);
@@ -33,8 +34,6 @@ void setup()
   // internally, this will display the splashscreen.
   display.display();
   delay(1000);
-
-  smartServo.moveServoToZero();
 }
 
 void printMsg(const String &s)
@@ -58,27 +57,27 @@ void printMsg(unsigned long l)
 
 bool isBtnPressed()
 {
-  if (millis() < 10000) return false;
+  if (millis() < 10000)
+    return false;
   return digitalRead(TOUCH_BTN_PIN) == HIGH;
 }
 void loop()
 {
   teaTimer.tick();
 
-  if (isBtnPressed())
+  if (isBtnPressed() && !teaTimer.isSteeping())
   {
-    printMsg("BUTTON");
     teaTimer.beginSteeping(5000);
   }
-  else if (!teaTimer.isSteeping())
-  {
-    printMsg("Default");
-  }
-  else
+  if (teaTimer.isSteeping())
   {
     auto r = teaTimer.remainingSteepTimeSeconds();
     printMsg(r);
   }
+  else
+  {
+    printMsg("Default");
+  }
 }
 
 void toggle_led()