Fix esp32 servo implementation, update rust service to work with it and support print vehicle

car-rs/src/grpcserver.rs

@@ -8,7 +8,7 @@ pub mod slam_controller_service {
 
 use std::{sync::Mutex, time::Duration};
 
-use car_rs::{Servo, Vehicle};
+use car_rs::Vehicle;
 use futures_util::StreamExt;
 use motor_control_service::car_control_server::CarControl;
 use tokio::time;

car-rs/src/lib.rs

@@ -1,5 +1,3 @@
-use std::sync::{Arc, Mutex};
-
 use serialport::SerialPort;
 
 mod lidar;
@@ -136,7 +134,7 @@ impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
         self.value = temp_value;
         let bytes_written = self
             .serial_port
-            .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]);
+            .write(&[self.channel, ((value + 1.) / 2. * 14. + 7.) as u8]);
         // TODO: Better error handling
         match bytes_written {
             Ok(size) => println!("{}", size),

car-rs/src/main.rs

@@ -1,5 +1,7 @@
-use car_rs::{Esp32SerialPwmServo, ServoVehicle};
+use std::net::SocketAddr;
-use clap::Parser;
+
+use car_rs::{Esp32SerialPwmServo, PrintVehicle, ServoVehicle, Vehicle};
+use clap::{Parser, ValueEnum};
 use grpcserver::{
     motor_control_service::car_control_server::CarControlServer, MotorControlService,
 };
@@ -8,6 +10,14 @@ use tonic::transport::Server;
 
 mod grpcserver;
 
+#[derive(Debug, Copy, Clone, PartialEq, Eq, PartialOrd, Ord, ValueEnum)]
+enum VehicleType {
+    /// Use esp32 to control the car connected via usb
+    Esp32Serial,
+    /// Debug vehicle to print changes to std out.
+    Print,
+}
+
 #[derive(Parser, Debug)]
 #[command(version, about, long_about = None)]
 struct Args {
@@ -19,28 +29,45 @@ struct Args {
 
     #[arg(short, long, default_value_t = 115200)]
     baud_rate: u32,
+
+    #[arg(value_enum, short, long, default_value_t = VehicleType::Print)]
+    vehicle_type: VehicleType,
 }
 
 #[tokio::main]
 async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let args = Args::parse();
     let addr = format!("[::1]:{}", &args.web_port).parse().unwrap();
-
+    match args.vehicle_type {
-    let mut steering_port = serialport::new(&args.serial_port, args.baud_rate)
+        VehicleType::Esp32Serial => {
-        .open_native()
+            let mut steering_port = serialport::new(&args.serial_port, args.baud_rate)
-        .expect("Could not open serial port");
+                .open_native()
-    steering_port.set_exclusive(false)?;
+                .expect("Could not open serial port");
-    let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate)
+            steering_port.set_exclusive(false)?;
-        .open_native()
+            let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate)
-        .expect("Could not open serial port");
+                .open_native()
-    throttle_port.set_exclusive(false)?;
+                .expect("Could not open serial port");
-    let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12);
+            throttle_port.set_exclusive(false)?;
-    let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18);
+            let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12);
-
+            let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18);
-    let motor_control = MotorControlService::new(ServoVehicle::new(steering_servo, throttle_servo));
+            create_service(ServoVehicle::new(steering_servo, throttle_servo), addr).await?;
-
+        }
-    let svc = CarControlServer::new(motor_control);
+        VehicleType::Print => {
-    Server::builder().add_service(svc).serve(addr).await?;
+            let vehicle = PrintVehicle::default();
+            create_service(vehicle, addr).await?;
+        }
+    }
 
     Ok(())
 }
+
+async fn create_service<T>(vehicle: T, addr: SocketAddr) -> Result<(), Box<dyn std::error::Error>>
+where
+    T: Vehicle + Send + Sync + 'static,
+{
+    let motor_control = MotorControlService::new(vehicle);
+
+    let svc = CarControlServer::new(motor_control);
+    Server::builder().add_service(svc).serve(addr).await?;
+    Ok(())
+}

esp32/README.md

@@ -4,25 +4,29 @@ The sketch takes simple input from serial (ESP32 microusb port), and converts th
 
 The protocol specification is as follows, assuming an array of bytes:
 
-
+| Byte Number |                                                                                                 Value Description |
-| Byte Number   | Value Description |
+| :---------- | ----------------------------------------------------------------------------------------------------------------: |
-| :---------- |  ----------: |
+| 0           |                            0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. |
-| 0 | 0 if Calibrating a servo. Higher values indicates channel number to set duty cycle on. |
+| 1           | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. |
-| 1 | If byte 0 = 0: number of servos to calibrate. Else, the new duty cycle value for the channel specified in byte 0. |
 
 When setting the duty cycle, the current min angle is 0, and the max angle is 255, which allows the entire byte to be used for setting the duty cycle.
 
 The table below describes the byte format for calibrating a servo. This array of bytes can be repeated for the given number of servos in byte 1:
 
-| Byte Number   | Value Description |
+| Byte Number |                                                                   Value Description |
-| :----------   |  ----------:      |
+| :---------- | ----------------------------------------------------------------------------------: |
-| 0             | Servo channel to set (this will be byte 0 when setting duty cycle, so don't use 0). |
+| 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 |
+| 1           |                                                             The pin number to setup |
 
 The min/max pulse widths are hardcoded to 1000us/2000us respectively.
 
 At the end of each loop, the entire array will be read, to flush any data that may cause issues later.
 
 Upcoming (TODO):
+
 - Use bit shift to allow 12 bits to be used for the duty cycle range, as there can only be a max of 16 channels anyway (4 bits).
 - Consider protobuf or msgpack for serialisation format, for more advanced use cases, and more maintainable communication formats (currently changing the message format will require changes to the entire protocol)
+
+## Deployment
+
+Sometimes you'll need to hold the boot button on the ESP32, then press upload (right arrow) on platformIO. Once the upload starts, you may release the boot button and wait for upload to complete.

esp32/platformio.ini

@@ -12,4 +12,4 @@
 platform = espressif32
 board = esp32doit-devkit-v1
 framework = arduino
-lib_deps = roboticsbrno/ServoESP32@^1.0.3
+monitor_speed = 115200

esp32/src/main.cpp

@@ -1,16 +1,8 @@
 #include <Arduino.h>
 
-#include <map>
-#include "Servo.h"
-
 // Min/max widths, used to calculate min/max duty cycles.
-#define MIN_PULSE_WIDTH 1000
+const int MIN_ANGLE = 12;
-#define MAX_PULSE_WIDTH 2000
+const int MAX_ANGLE = 26;
-
-#define MIN_ANGLE 0
-#define MAX_ANGLE 255
-
-std::map<uint8_t, Servo *> servos;
 
 void setup()
 {
@@ -24,19 +16,26 @@ void setupServos(uint8_t size, uint8_t *calibrationValues)
     {
         for (int i = 0; i < size; i += 2)
         {
-            Servo *newServo = new Servo();
+            int channel = calibrationValues[i];
-            newServo->attach(calibrationValues[i + 1], calibrationValues[i], MIN_ANGLE, MAX_ANGLE, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
+            int pin = calibrationValues[i + 1];
-            servos.insert(std::make_pair(calibrationValues[i], newServo));
+            pinMode(pin, OUTPUT);
+            ledcSetup(channel, 50, 8);
+            ledcAttachPin(pin, channel);
         }
     }
 }
 
 void modifyServo(uint8_t channel, uint8_t newAngle)
 {
-    if (servos.count(channel) > 0)
+    if (newAngle > MAX_ANGLE)
     {
-        servos[channel]->write(newAngle);
+        newAngle = MAX_ANGLE;
     }
+    else if (newAngle < MIN_ANGLE)
+    {
+        newAngle = MIN_ANGLE;
+    }
+    ledcWrite(channel, newAngle);
 }
 
 void loop()

pycar/requirements.txt

@@ -6,7 +6,8 @@ paho-mqtt
 u-msgpack-python
 grpcio-tools
 rplidar
-breezyslam
+# breezyslam
 pyzmq
 gpiozero
 pigpio
+esptool