From 5fab95c8e22468f5a935f3931fa115c4ee5cbba0 Mon Sep 17 00:00:00 2001
From: Noa Fabbricotti <gitea@fabbricotti.nl>
Date: Fri, 27 Mar 2026 14:42:04 +0100
Subject: [PATCH] Edit ultrasonic.rs structure and implement in main.rs

---
 src/main.rs                  | 12 ++++++
 src/moduleTests/motorTest.rs | 79 +++++++++++++++++-------------------
 src/ultrasonic.rs            | 72 ++++++++++++--------------------
 3 files changed, 76 insertions(+), 87 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index d42e06f..90dc04c 100644
--- a/src/main.rs
+++ b/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
diff --git a/src/moduleTests/motorTest.rs b/src/moduleTests/motorTest.rs
index 2579f61..177bf9b 100644
--- a/src/moduleTests/motorTest.rs
+++ b/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(())
-}
\ No newline at end of file
+}
diff --git a/src/ultrasonic.rs b/src/ultrasonic.rs
index 33fdeee..2a5c438 100644
--- a/src/ultrasonic.rs
+++ b/src/ultrasonic.rs
@@ -1,53 +1,35 @@
 use hc_sr04::{HcSr04, Unit};
 
-let mut ultrasonic_front = HcSr04::new(
-    7,          // TRIGGER
-    1,          // ECHO
-    Some(23_f32) 
-).unwrap();
-
-let mut ultrasonic_left = HcSr04::new(
-    0,
-    4,
-    Some(23_f32) 
-).unwrap();
-
-let mut ultrasonic_right = HcSr04::new(
-    2,
-    5,
-    Some(23_f32) 
-).unwrap();
-
-let mut ultrasonic_rear = HcSr04::new(
-    3,
-    6,
-    Some(23_f32) 
-).unwrap();
-
-fn get_front_dist() -> Option<i32> {
-	match ultrasonic_front.measure_distance(Unit::Meters).unwrap() {
-    		Some(dist) => Some(dist),
-    		None => None,
-	}
+pub struct Ultrasonic {
+    front: HcSr04,
+    left: HcSr04,
+    right: HcSr04,
+    rear: HcSr04,
 }
 
-fn get_left_dist() -> Option<i32> {
-        match ultrasonic_left.measure_distance(Unit::Meters).unwrap() {
-                Some(dist) => Some(dist),
-                None => None,
+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(),
         }
-}
+    }
 
-fn get_right_dist() -> Option<i32> {
-        match ultrasonic_right.measure_distance(Unit::Meters).unwrap() {
-                Some(dist) => Some(dist),
-                None => None,
-        }
-}
+    pub fn get_front_dist(&mut self) -> Option<i32> {
+        self.front.measure_distance(Unit::Meters).ok().flatten()
+    }
 
-fn get_rear_dist() -> Option<i32> {
-        match ultrasonic_rear.measure_distance(Unit::Meters).unwrap() {
-                Some(dist) => Some(dist),
-                None => None,
-        }
+    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()
+    }
 }