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

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);
     }
 }