From b72781e32295c10dcb511f218c09ae141706ed51 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Tue, 8 Sep 2020 21:23:21 +0930
Subject: [PATCH] Initial PlatformIO code.

---
 esp32/README.md    |  1 -
 esp32/src/main.cpp | 32 ++++++++++++++++++++++++++------
 2 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/esp32/README.md b/esp32/README.md
index 051ac24..a206c6b 100644
--- a/esp32/README.md
+++ b/esp32/README.md
@@ -16,7 +16,6 @@ The table below describes the byte format for calibrating a servo. This array of
 | :----------   |  ----------:      |
 | 0             | Servo channel to set (this will be byte 0 when setting duty cycle, so don't use 0). |
 | 1             | The pin number to setup |
-| 2             | The frequency for the pin |
 
 The min/max pulse widths are hardcoded to 1000us/2000us respectively. 
 
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index a01dbc1..e189f14 100644
--- a/esp32/src/main.cpp
+++ b/esp32/src/main.cpp
@@ -3,7 +3,14 @@
 #include <map>
 #include "Servo.h"
 
-std::map<uint8_t, Servo> servos;
+// Min/max widths, used to calculate min/max duty cycles.
+#define MIN_PULSE_WIDTH 1000
+#define MAX_PULSE_WIDTH 2000
+
+#define MIN_ANGLE 0
+#define MAX_ANGLE 255
+
+std::map<uint8_t, Servo *> servos;
 
 void setup()
 {
@@ -15,13 +22,22 @@ void setupServos(uint8_t size, uint8_t *calibrationValues)
     // We assume there are 3 bytes per servo. Ignore if there aren't.
     if (size % 3 == 0)
     {
-        for (int i = 0; i < size; i += 3)
+        for (int i = 0; i < size; i += 2)
         {
-            servos.insert(std::make_pair(calibrationValues[i], new Servo()));
+            Servo *newServo = new Servo();
+            newServo->attach(calibrationValues[i + 1], calibrationValues[i], MIN_ANGLE, MAX_ANGLE, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+            servos.insert(std::make_pair(calibrationValues[i], newServo));
         }
     }
 }
 
+void modifyServo(uint8_t channel, uint8_t newAngle)
+{
+    if(servos.count(channel) > 0){
+        servos[channel]->write(newAngle);
+    } 
+}
+
 void loop()
 {
     uint8_t *header = new uint8_t[2];
@@ -29,11 +45,15 @@ void loop()
 
     if (header[0] == 0)
     {
-        uint8_t *calibration = new uint8_t[3 * header[1]];
+        uint8_t *calibration = new uint8_t[2 * header[1]];
         Serial.readBytes(calibration, header[1]);
         setupServos(header[1], calibration);
-        delete calibration;
+        delete [] calibration;
+    }
+    else
+    {
+        modifyServo(header[0], header[1]);
     }
 
-    delete header;
+    delete [] header;
 }
\ No newline at end of file