Edit ultrasonic.rs structure and implement in main.rs

TODO.md

@@ -0,0 +1 @@
+- [ ] Improve config file as Rust is not ideal.

src/config.rs

@@ -0,0 +1,19 @@
+pub struct Config {
+    // Wheel GPIO pins.
+    pub left1: u8,
+    pub left2: u8,
+    pub right1: u8,
+    pub right2: u8
+}
+
+impl Config {
+    pub fn new() -> Self {
+        Self {
+            // Wheel GPIO pins.
+            left1: 27,
+            left2: 22,
+            right1: 23,
+            right2: 24
+        }
+    }
+}

src/main.rs

@@ -1,34 +1,38 @@
-use rppal::gpio::{Gpio, OutputPin};
+// External libraries.
+use rppal::gpio::{Gpio};
 use rppal::pwm::{Pwm, Channel, Polarity};
 use std::{error::Error, thread, time::Duration};
 
-struct Motor {
-    in1: OutputPin,
-    in2: OutputPin,
-    pwm: Pwm,
-}
+// Own libraries.
+mod motors;
+use motors::Motor;
 
-impl Motor {
-    fn forward(&mut self, speed: f64) {
-        self.in1.set_high();
-        self.in2.set_low();
-        self.pwm.set_duty_cycle(speed).unwrap();
-    }
+// Config
+mod config;
+use config::Config;
 
-    fn backward(&mut self, speed: f64) {
-        self.in1.set_low();
-        self.in2.set_high();
-        self.pwm.set_duty_cycle(speed).unwrap();
-    }
-
-    fn stop(&mut self) {
-        self.in1.set_low();
-        self.in2.set_low();
-        self.pwm.set_duty_cycle(0.0).unwrap();
-    }
-}
+mod ultrasonic;
 
 fn main() -> Result<(), Box<dyn Error>> {
+    let config = Config::new();
+    println!(
+        "Starting with GPIO pins:\n - Motors\n   - Right 1: {}\n   - Right 2: {}\n   - Left 1: {}\n   - Left 2: {}",
+        config.right1,
+        config.right2,
+        config.left1,
+        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 std::thread;
-use std::time::Duration;
+use rppal::gpio::{Gpio};
+use rppal::pwm::{Pwm, Channel, Polarity};
+use std::{error::Error, thread, time::Duration};
 
-// struct L298n {
-//   in1: Gpio,
-//   in2: Gpio,
-//   ena: Gpio,
-// }
+mod motors;
+use motors::Motor;
 
-fn main() -> Result<(), std::io::Error> {
-    let mut motor_front_left = L298n::new(0, 0, 0);
-    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);
+mod config;
+use config::Config;
 
-    // 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
-
-    motor_front_right.forward()?;
-    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()?;
+    // Stop both motors
+    left.stop();
+    right.stop();
 
     Ok(())
 }

src/motors.rs

@@ -0,0 +1,29 @@
+use rppal::gpio::OutputPin;
+use rppal::pwm::Pwm;
+
+pub struct Motor {
+    pub in1: OutputPin,
+    pub in2: OutputPin,
+    pub pwm: Pwm,
+}
+
+
+impl Motor {
+    pub fn forward(&mut self, speed: f64) {
+        self.in1.set_high();
+        self.in2.set_low();
+        self.pwm.set_duty_cycle(speed).unwrap();
+    }
+
+    pub fn backward(&mut self, speed: f64) {
+        self.in1.set_low();
+        self.in2.set_high();
+        self.pwm.set_duty_cycle(speed).unwrap();
+    }
+
+    pub fn stop(&mut self) {
+        self.in1.set_low();
+        self.in2.set_low();
+        self.pwm.set_duty_cycle(0.0).unwrap();
+    }
+}

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()
+    }
+}