From 3daa81571032a0d2f854871610803b1c433fa5de Mon Sep 17 00:00:00 2001
From: Piv <18462828+Piv200@users.noreply.github.com>
Date: Sun, 2 Oct 2022 15:44:38 +1030
Subject: [PATCH] Add single steering/throttle grpc methods, add bounds checks
 to set servo value

---
 car-rs/src/grpcserver.rs | 12 ++++++++++--
 car-rs/src/lib.rs        | 14 ++++++++++----
 2 files changed, 20 insertions(+), 6 deletions(-)

diff --git a/car-rs/src/grpcserver.rs b/car-rs/src/grpcserver.rs
index 67d4961..1c46876 100644
--- a/car-rs/src/grpcserver.rs
+++ b/car-rs/src/grpcserver.rs
@@ -39,14 +39,22 @@ impl<T: Vehicle + Send + Sync + 'static> CarControl for MotorControlService<T> {
         &self,
         _request: Request<ThrottleRequest>,
     ) -> Result<Response<ThrottleResponse>, Status> {
-        unimplemented!()
+        self.vehicle
+            .lock()
+            .unwrap()
+            .set_throttle(_request.into_inner().throttle as f64);
+        Ok(Response::new(ThrottleResponse { throttle_set: true }))
     }
 
     async fn set_steering(
         &self,
         _request: Request<SteeringRequest>,
     ) -> Result<Response<SteeringResponse>, Status> {
-        unimplemented!()
+        self.vehicle
+            .lock()
+            .unwrap()
+            .set_steering(_request.into_inner().steering as f64);
+        Ok(Response::new(SteeringResponse { steering_set: true }))
     }
 
     async fn stream_vehicle_2d(
diff --git a/car-rs/src/lib.rs b/car-rs/src/lib.rs
index 019bf92..cf22e2b 100644
--- a/car-rs/src/lib.rs
+++ b/car-rs/src/lib.rs
@@ -123,7 +123,14 @@ impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
     }
 
     fn set_value(&mut self, value: f64) {
-        self.value = value;
+        let mut temp_value = value;
+        // TODO: Panic when out of bounds?
+        if temp_value < -1. {
+            temp_value = -1.;
+        } else if temp_value > 1. {
+            temp_value = 1.;
+        }
+        self.value = temp_value;
         let bytes_written = self
             .serial_port
             .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]);
@@ -150,9 +157,8 @@ impl<T: Servo> ServoVehicle<T> {
 }
 
 impl<T: Servo> Vehicle for ServoVehicle<T> {
-    // TODO: Set duty cycle correctly
     fn get_throttle(&self) -> f64 {
-        unimplemented!()
+        self.throttle_servo.get_value()
     }
 
     fn set_throttle(&mut self, throttle: f64) {
@@ -160,7 +166,7 @@ impl<T: Servo> Vehicle for ServoVehicle<T> {
     }
 
     fn get_steering(&self) -> f64 {
-        unimplemented!()
+        self.steering_servo.get_value()
     }
 
     fn set_steering(&mut self, steering: f64) {