Edit ultrasonic.rs structure and implement in main.rs

src/main.rs

@@ -11,6 +11,8 @@ use motors::Motor;
 mod config;
 use config::Config;
 
+mod ultrasonic;
+
 fn main() -> Result<(), Box<dyn Error>> {
     let config = Config::new();
     println!(
@@ -21,6 +23,16 @@ fn main() -> Result<(), Box<dyn Error>> {
         config.left2
     );
 
+    // Read front distance
+
+    let mut ultrasonic = Ultrasonic::new();
+
+    if let Some(dist) = ultrasonic.get_front_dist() {
+        println!("Front distance: {}", dist);
+    } else {
+        println!("No reading");
+    }
+
     let gpio = Gpio::new()?;
 
     // LEFT

src/moduleTests/motorTest.rs

@@ -1,50 +1,45 @@
-use raslib::L298n;
+use rppal::gpio::{Gpio};
-use std::thread;
+use rppal::pwm::{Pwm, Channel, Polarity};
-use std::time::Duration;
+use std::{error::Error, thread, time::Duration};
 
-// struct L298n {
+mod motors;
-//   in1: Gpio,
+use motors::Motor;
-//   in2: Gpio,
-//   ena: Gpio,
-// }
 
-fn main() -> Result<(), std::io::Error> {
+mod config;
-    let mut motor_front_left = L298n::new(0, 0, 0);
+use config::Config;
-    let mut motor_front_right = L298n::new(0, 0, 0);
-    let mut motor_rear_left = L298n::new(0, 0, 0);
-    let mut motor_rear_right = L298n::new(0, 0, 0);
 
-    // Front left test
+fn main() -> Result<(), Box<dyn Error>> {
+    let config = Config::new();
+    let gpio = Gpio::new()?;
 
-    motor_front_left.forward()?;
+    let left = Motor {
+        in1: gpio.get(27)?.into_output(),
+        in2: gpio.get(22)?.into_output(),
+        pwm: Pwm::with_frequency(Channel::Pwm0, 1000.0, 0.0, Polarity::Normal, true)?,
+    };
+
+    let right = Motor {
+        in1: gpio.get(23)?.into_output(),
+        in2: gpio.get(24)?.into_output(),
+        pwm: Pwm::with_frequency(Channel::Pwm1, 1000.0, 0.0, Polarity::Normal, true)?,
+    };
+
+    let mut left = left;
+    let mut right = right;
+
+    // Forwards 3 seconds
+    left.forward(1.0);
+    right.forward(1.0);
+    thread::sleep(Duration::from_secs(3));
+
+    // Backwards 3 seconds
+    left.backward(0.6);
+    right.backward(0.6);
     thread::sleep(Duration::from_secs(2));
-    motor_front_left.backward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_front_left.stop()?;
 
-    // Front right test
+    // Stop both motors
-
+    left.stop();
-    motor_front_right.forward()?;
+    right.stop();
-    thread::sleep(Duration::from_secs(2));
-    motor_front_right.backward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_front_right.stop()?;
-
-    // Rear left test
-
-    motor_rear_left.forward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_rear_left.backward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_rear_left.stop()?;
-
-    // Rear right test
-
-    motor_rear_right.forward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_rear_right.backward()?;
-    thread::sleep(Duration::from_secs(2));
-    motor_rear_right.stop()?;
 
     Ok(())
 }

src/ultrasonic.rs

@@ -0,0 +1,35 @@
+use hc_sr04::{HcSr04, Unit};
+
+pub struct Ultrasonic {
+    front: HcSr04,
+    left: HcSr04,
+    right: HcSr04,
+    rear: HcSr04,
+}
+
+impl Ultrasonic {
+    pub fn new() -> Self {
+        Self {
+            front: HcSr04::new(7, 1, Some(23.0)).unwrap(),
+            left: HcSr04::new(0, 4, Some(23.0)).unwrap(),
+            right: HcSr04::new(2, 5, Some(23.0)).unwrap(),
+            rear: HcSr04::new(3, 6, Some(23.0)).unwrap(),
+        }
+    }
+
+    pub fn get_front_dist(&mut self) -> Option<i32> {
+        self.front.measure_distance(Unit::Meters).ok().flatten()
+    }
+
+    pub fn get_left_dist(&mut self) -> Option<i32> {
+        self.left.measure_distance(Unit::Meters).ok().flatten()
+    }
+
+    pub fn get_right_dist(&mut self) -> Option<i32> {
+        self.right.measure_distance(Unit::Meters).ok().flatten()
+    }
+
+    pub fn get_rear_dist(&mut self) -> Option<i32> {
+        self.rear.measure_distance(Unit::Meters).ok().flatten()
+    }
+}