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 clap::Parser;
+use std::net::SocketAddr;
+
+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();
-
-    let mut steering_port = serialport::new(&args.serial_port, args.baud_rate)
-        .open_native()
-        .expect("Could not open serial port");
-    steering_port.set_exclusive(false)?;
-    let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate)
-        .open_native()
-        .expect("Could not open serial port");
-    throttle_port.set_exclusive(false)?;
-    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));
-
-    let svc = CarControlServer::new(motor_control);
-    Server::builder().add_service(svc).serve(addr).await?;
+    match args.vehicle_type {
+        VehicleType::Esp32Serial => {
+            let mut steering_port = serialport::new(&args.serial_port, args.baud_rate)
+                .open_native()
+                .expect("Could not open serial port");
+            steering_port.set_exclusive(false)?;
+            let mut throttle_port = serialport::new(&args.serial_port, args.baud_rate)
+                .open_native()
+                .expect("Could not open serial port");
+            throttle_port.set_exclusive(false)?;
+            let steering_servo = Esp32SerialPwmServo::new(steering_port, 1, 12);
+            let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18);
+            create_service(ServoVehicle::new(steering_servo, throttle_servo), addr).await?;
+        }
+        VehicleType::Print => {
+            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(())
+}