From 34fbc40ebb0b4a7c67bb0cddb30b69dc343f8795 Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Sun, 22 Jan 2023 15:05:32 +1030
Subject: [PATCH] Initialise esp32 pwm, fix esp32 initial setup as it uses 2
 bytes

---
 car-rs/src/lib.rs  | 8 +++++---
 car-rs/src/main.rs | 1 -
 esp32/src/main.cpp | 4 ++--
 3 files changed, 7 insertions(+), 6 deletions(-)

diff --git a/car-rs/src/lib.rs b/car-rs/src/lib.rs
index 3058605..80da653 100644
--- a/car-rs/src/lib.rs
+++ b/car-rs/src/lib.rs
@@ -99,12 +99,14 @@ pub struct Esp32SerialPwmServo<T: SerialPort> {
 
 impl<T: SerialPort> Esp32SerialPwmServo<T> {
     pub fn new(serial_port: Arc<Mutex<T>>, channel: u8, pin: u8) -> Esp32SerialPwmServo<T> {
-        Esp32SerialPwmServo {
+        let mut servo = Esp32SerialPwmServo {
             serial_port,
             value: 0.,
             channel,
             pin,
-        }
+        };
+        servo.init_pwm();
+        servo
     }
 }
 
@@ -115,7 +117,7 @@ impl<T: SerialPort> Esp32SerialPwmServo<T> {
             .lock()
             .unwrap()
             .write(&[0, 1, self.channel, self.pin]);
-        // TODO: Better error handling
+        // TODO: Better error handling (even anyhow would be better)
         match bytes_written {
             Ok(size) => println!("{}", size),
             Err(err) => eprintln!("{}", err),
diff --git a/car-rs/src/main.rs b/car-rs/src/main.rs
index 1b5b4af..b66f42a 100644
--- a/car-rs/src/main.rs
+++ b/car-rs/src/main.rs
@@ -13,7 +13,6 @@ mod grpcserver;
 async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let addr = "[::1]:10000".parse().unwrap();
 
-    // TODO: Do these actually need to be shared since they're both on one port?
     let serial_port = serialport::new("", 32400)
         .open_native()
         .expect("Could not open serial port");
diff --git a/esp32/src/main.cpp b/esp32/src/main.cpp
index 60e892d..48a54dc 100644
--- a/esp32/src/main.cpp
+++ b/esp32/src/main.cpp
@@ -19,8 +19,8 @@ void setup()
 
 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)
+    // We assume there are 2 bytes per servo [channel number, pin number]. Ignore if there aren't.
+    if (size % 2 == 0)
     {
         for (int i = 0; i < size; i += 2)
         {