Setup 2d streaming with vehicle, add servo vehicle and some servo refactor/implementation

car-rs/src/grpcserver.rs

@@ -2,10 +2,7 @@ pub mod motor_control_service {
     tonic::include_proto!("motor_control");
 }
 
-use std::{
-    marker::{self, PhantomData},
-    time::Duration,
-};
+use std::{sync::Mutex, time::Duration};
 
 use car_rs::{Servo, Vehicle};
 use futures_util::StreamExt;
@@ -23,12 +20,16 @@ pub struct MotorControlService<T>
 where
     T: Vehicle,
 {
-    vehicle: T,
+    // TODO: Any better way than mutex? need it over refcell to implement send, and need a smart pointer
+    //  for interior mutability (since the generated protobuf functions aren't mut)
+    vehicle: Mutex<T>,
 }
 
 impl<T: Vehicle> MotorControlService<T> {
     pub fn new(vehicle: T) -> MotorControlService<T> {
-        MotorControlService { vehicle: vehicle }
+        MotorControlService {
+            vehicle: Mutex::new(vehicle),
+        }
     }
 }
 
@@ -54,15 +55,12 @@ impl<T: Vehicle + Send + Sync + 'static> CarControl for MotorControlService<T> {
     ) -> Result<Response<Vehicle2DResponse>, Status> {
         let mut stream = request.into_inner();
         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);
+            let mut vehicle = self.vehicle.lock().unwrap();
+            vehicle.set_throttle(req.throttle.unwrap().throttle as f64);
+            vehicle.set_steering(req.steering.unwrap().steering as f64);
         }
 
-        // TODO: Stop the vehicle...
-        // self.vehicle.stop();
-        self.vehicle.set_throttle(0.);
+        self.vehicle.lock().unwrap().set_throttle(0.);
 
         Ok(Response::new(Vehicle2DResponse {}))
     }

car-rs/src/lib.rs

@@ -1,20 +1,27 @@
 use serialport::SerialPort;
 
+// TODO: Should be returning results in these traits
 pub trait Servo {
-    fn get_duty_cycle(&self) -> f64;
-    // TODO: Some kind of error handling here, in case we fail to set the duty cycle
-    fn set_duty_cycle(&self, pwm: f64);
+    fn get_value(&self) -> f64;
 
-    fn get_frequency(&self) -> f64;
+    fn set_value(&mut self, value: f64);
 
-    fn set_frequency(&self, frequency: f64);
+    fn min(&mut self) {
+        self.set_value(-1.);
+    }
+    fn mid(&mut self) {
+        self.set_value(0.);
+    }
+    fn max(&mut self) {
+        self.set_value(1.);
+    }
 }
 
 pub trait Vehicle {
     fn get_throttle(&self) -> f64;
-    fn set_throttle(&self, throttle: f64);
+    fn set_throttle(&mut self, throttle: f64);
     fn get_steering(&self) -> f64;
-    fn set_steering(&self, steering: f64);
+    fn set_steering(&mut self, steering: f64);
 }
 #[cfg(feature = "rppal")]
 pub mod rppal {
@@ -22,11 +29,31 @@ pub mod rppal {
 
     pub struct RpiPwmServo {
         pwm: Pwm,
+        min_duty_cycle: f64,
+        duty_cycle_range: f64,
+        value: f64,
+        frame_width: f64,
     }
 
     impl RpiPwmServo {
         pub fn new(pwm: Pwm) -> RpiPwmServo {
-            RpiPwmServo { pwm }
+            RpiPwmServo::new(pwm, 1000000)
+        }
+
+        pub fn new(pwm: Pwm, min_pulse_width: f64) -> RpiPwmServo {}
+
+        pub fn new(
+            pwm: Pwm,
+            min_pulse_width: f64,
+            max_pulse_width: f64,
+            frame_width: f64,
+        ) -> RpiPwmServo {
+            RpiPwmServo {
+                pwm,
+                min_duty_cycle: min_pulse_width / frame_width,
+                duty_cycle_range: (max_pulse_width - min_pulse_width) / frame_width,
+                frame_width,
+            }
         }
     }
 
@@ -63,56 +90,80 @@ pub mod rppal {
 
 pub struct Esp32SerialPwmServo<T: SerialPort> {
     serial_port: T,
+    value: f64,
+    channel: u8,
+    pin: u8,
 }
 
 impl<T: SerialPort> Esp32SerialPwmServo<T> {
-    pub fn new(serial_port: T) -> Esp32SerialPwmServo<T> {
-        Esp32SerialPwmServo { serial_port }
+    pub fn new(serial_port: T, channel: u8, pin: u8) -> Esp32SerialPwmServo<T> {
+        Esp32SerialPwmServo {
+            serial_port,
+            value: 0.,
+            channel,
+            pin,
+        }
+    }
+}
+
+impl<T: SerialPort> Esp32SerialPwmServo<T> {
+    fn init_pwm(&mut self) {
+        let bytes_written = self.serial_port.write(&[0, 1, self.channel, self.pin]);
+        // TODO: Better error handling
+        match bytes_written {
+            Ok(size) => println!("{}", size),
+            Err(err) => eprintln!("{}", err),
+        }
     }
 }
 
 impl<T: SerialPort> Servo for Esp32SerialPwmServo<T> {
-    fn get_duty_cycle(&self) -> f64 {
-        todo!()
+    fn get_value(&self) -> f64 {
+        self.value
     }
 
-    fn set_duty_cycle(&self, pwm: f64) {
-        todo!()
-    }
-
-    fn get_frequency(&self) -> f64 {
-        todo!()
-    }
-
-    fn set_frequency(&self, frequency: f64) {
-        todo!()
+    fn set_value(&mut self, value: f64) {
+        self.value = value;
+        let bytes_written = self
+            .serial_port
+            .write(&[self.channel, ((value + 1.) / 2. * 255.) as u8]);
+        // TODO: Better error handling
+        match bytes_written {
+            Ok(size) => println!("{}", size),
+            Err(err) => eprintln!("{}", err),
+        }
     }
 }
 
 pub struct ServoVehicle<T: Servo> {
-    servo: T,
+    steering_servo: T,
+    throttle_servo: T,
 }
 
 impl<T: Servo> ServoVehicle<T> {
-    pub fn new(servo: T) -> ServoVehicle<T> {
-        ServoVehicle { servo }
+    pub fn new(steering_servo: T, throttle_servo: T) -> ServoVehicle<T> {
+        ServoVehicle {
+            steering_servo,
+            throttle_servo,
+        }
     }
 }
 
 impl<T: Servo> Vehicle for ServoVehicle<T> {
+    // TODO: Set duty cycle correctly
     fn get_throttle(&self) -> f64 {
-        todo!()
+        unimplemented!()
     }
 
-    fn set_throttle(&self, throttle: f64) {
-        todo!()
+    fn set_throttle(&mut self, throttle: f64) {
+        self.throttle_servo.set_value(throttle);
     }
 
     fn get_steering(&self) -> f64 {
-        todo!()
+        unimplemented!()
     }
 
-    fn set_steering(&self, steering: f64) {
-        todo!()
+    fn set_steering(&mut self, steering: f64) {
+        self.steering_servo.set_value(steering);
     }
 }

car-rs/src/main.rs

@@ -11,12 +11,17 @@ mod grpcserver;
 async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let addr = "[::1]:10000".parse().unwrap();
 
-    let serial_port = "";
-    let serial_port = serialport::new(serial_port, 32400)
+    // 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");
-    let servo = Esp32SerialPwmServo::new(serial_port);
-    let motor_control = MotorControlService::new(ServoVehicle::new(servo));
+    let throttle_port = serialport::new("", 32400)
+        .open_native()
+        .expect("Could not open serial port");
+    let servo = Esp32SerialPwmServo::new(serial_port, 1, 12);
+    let throttle_servo = Esp32SerialPwmServo::new(throttle_port, 2, 18);
+
+    let motor_control = MotorControlService::new(ServoVehicle::new(servo, throttle_servo));
 
     let svc = CarControlServer::new(motor_control);
     Server::builder().add_service(svc).serve(addr).await?;