From 42477b3542a16e04e692c006c7b4f3897f6ca12a Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Sat, 1 Oct 2022 23:15:53 +0930
Subject: [PATCH] Minor refactor of vehicle and servo to simplify traits

---
 car-rs/src/grpcserver.rs | 28 ++++++++++++++++++++++-----
 car-rs/src/lib.rs        | 42 +++++++++++++++++++++++++++++++++++++---
 car-rs/src/main.rs       |  9 ++++++++-
 3 files changed, 70 insertions(+), 9 deletions(-)

diff --git a/car-rs/src/grpcserver.rs b/car-rs/src/grpcserver.rs
index 8642591..eb08eb9 100644
--- a/car-rs/src/grpcserver.rs
+++ b/car-rs/src/grpcserver.rs
@@ -2,7 +2,10 @@ pub mod motor_control_service {
     tonic::include_proto!("motor_control");
 }
 
-use std::time::Duration;
+use std::{
+    marker::{self, PhantomData},
+    time::Duration,
+};
 
 use car_rs::{Servo, Vehicle};
 use futures_util::StreamExt;
@@ -16,10 +19,21 @@ use self::motor_control_service::{
 };
 
 #[derive(Debug)]
-pub struct MotorControlService {}
+pub struct MotorControlService<T>
+where
+    T: Vehicle,
+{
+    vehicle: T,
+}
+
+impl<T: Vehicle> MotorControlService<T> {
+    pub fn new(vehicle: T) -> MotorControlService<T> {
+        MotorControlService { vehicle: vehicle }
+    }
+}
 
 #[tonic::async_trait]
-impl CarControl for MotorControlService {
+impl<T: Vehicle + Send + Sync + 'static> CarControl for MotorControlService<T> {
     async fn set_throttle(
         &self,
         _request: Request<ThrottleRequest>,
@@ -39,12 +53,16 @@ impl CarControl for MotorControlService {
         request: Request<Streaming<Vehicle2DRequest>>,
     ) -> Result<Response<Vehicle2DResponse>, Status> {
         let mut stream = request.into_inner();
-        while let Ok(Some(req)) = time::timeout(Duration::from_secs(3), stream.next()).await {
-            // TODO: Set vehicle steering/throttle
+        while let Ok(Some(Ok(req))) = time::timeout(Duration::from_secs(3), stream.next()).await {
+            self.vehicle
+                .set_throttle(req.throttle.unwrap().throttle as f64);
+            self.vehicle
+                .set_steering(req.steering.unwrap().steering as f64);
         }
 
         // TODO: Stop the vehicle...
         // self.vehicle.stop();
+        self.vehicle.set_throttle(0.);
 
         Ok(Response::new(Vehicle2DResponse {}))
     }
diff --git a/car-rs/src/lib.rs b/car-rs/src/lib.rs
index 1ecc5da..8e40783 100644
--- a/car-rs/src/lib.rs
+++ b/car-rs/src/lib.rs
@@ -10,9 +10,11 @@ pub trait Servo {
     fn set_frequency(&self, frequency: f64);
 }
 
-pub trait Vehicle<T: Servo> {
-    fn get_throttle_servo() -> T;
-    fn get_steering_servo() -> T;
+pub trait Vehicle {
+    fn get_throttle(&self) -> f64;
+    fn set_throttle(&self, throttle: f64);
+    fn get_steering(&self) -> f64;
+    fn set_steering(&self, steering: f64);
 }
 #[cfg(feature = "rppal")]
 pub mod rppal {
@@ -63,6 +65,12 @@ pub struct Esp32SerialPwmServo<T: SerialPort> {
     serial_port: T,
 }
 
+impl<T: SerialPort> Esp32SerialPwmServo<T> {
+    pub fn new(serial_port: T) -> Esp32SerialPwmServo<T> {
+        Esp32SerialPwmServo { serial_port }
+    }
+}
+
 impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
     fn get_duty_cycle(&self) -> f64 {
         todo!()
@@ -80,3 +88,31 @@ impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
         todo!()
     }
 }
+
+pub struct ServoVehicle<T: Servo> {
+    servo: T,
+}
+
+impl<T: Servo> ServoVehicle<T> {
+    pub fn new(servo: T) -> ServoVehicle<T> {
+        ServoVehicle { servo }
+    }
+}
+
+impl<T: Servo> Vehicle for ServoVehicle<T> {
+    fn get_throttle(&self) -> f64 {
+        todo!()
+    }
+
+    fn set_throttle(&self, throttle: f64) {
+        todo!()
+    }
+
+    fn get_steering(&self) -> f64 {
+        todo!()
+    }
+
+    fn set_steering(&self, steering: f64) {
+        todo!()
+    }
+}
diff --git a/car-rs/src/main.rs b/car-rs/src/main.rs
index e1cc45a..31f613c 100644
--- a/car-rs/src/main.rs
+++ b/car-rs/src/main.rs
@@ -1,6 +1,8 @@
+use car_rs::{Esp32SerialPwmServo, ServoVehicle};
 use grpcserver::{
     motor_control_service::car_control_server::CarControlServer, MotorControlService,
 };
+
 use tonic::transport::Server;
 
 mod grpcserver;
@@ -9,7 +11,12 @@ mod grpcserver;
 async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let addr = "[::1]:10000".parse().unwrap();
 
-    let motor_control = MotorControlService {};
+    let serial_port = "";
+    let serial_port = serialport::new(serial_port, 32400)
+        .open_native()
+        .expect("Could not open serial port");
+    let servo = Esp32SerialPwmServo::new(serial_port);
+    let motor_control = MotorControlService::new(ServoVehicle::new(servo));
 
     let svc = CarControlServer::new(motor_control);
     Server::builder().add_service(svc).serve(addr).await?;