diff --git a/Cargo.toml b/Cargo.toml index 21e5ea6..0bf2e47 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4,11 +4,13 @@ members = [ "mightybuga_bsc", "libs/logging", "libs/engine", + "libs/process_calibration", "libs/timer_based_buzzer_interface", "libs/hal_button", "libs/hal_encoder_stm32f1xx", "apps/hello_world", "apps/line_follower", + "apps/line_sensor_simulator", ] resolver = "2" diff --git a/apps/line_sensor_simulator/Cargo.toml b/apps/line_sensor_simulator/Cargo.toml new file mode 100644 index 0000000..b112bd8 --- /dev/null +++ b/apps/line_sensor_simulator/Cargo.toml @@ -0,0 +1,9 @@ +[package] +name = "line_sensor_simulator" +version = "0.1.0" +edition = "2021" + +[dependencies] +process_calibration = { path = "../../libs/process_calibration" } + +rand = "0.8" \ No newline at end of file diff --git a/apps/line_sensor_simulator/README.md b/apps/line_sensor_simulator/README.md new file mode 100644 index 0000000..55d7c05 --- /dev/null +++ b/apps/line_sensor_simulator/README.md @@ -0,0 +1,27 @@ +# Line sensor simulator + +This apps simulates the calibration of the light sensor for implementing +the calibration logic without the need of the hardware. + +Using the calibration process at `libs/process_calibration`. + +``` +$ cargo xtask run app line_sensor_simulator +Starting calibration simulation... +Sample: [2750, 2750, 1450, 150, 150, 1450, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [2750, 1450, 150, 150, 1450, 2750, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 1450, 150, 150] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Sample: [150, 150, 1450, 2750, 2750, 2750, 2750, 2750] + +Calibration results: +Min values: [150, 150, 150, 150, 150, 1450, 150, 150] +Max values: [2750, 2750, 2750, 2750, 2750, 2750, 2750, 2750] +Thresholds: [1450, 1450, 1450, 1450, 1450, 2100, 1450, 1450] + +``` \ No newline at end of file diff --git a/apps/line_sensor_simulator/src/main.rs b/apps/line_sensor_simulator/src/main.rs new file mode 100644 index 0000000..21f9015 --- /dev/null +++ b/apps/line_sensor_simulator/src/main.rs @@ -0,0 +1,38 @@ +mod simulator; + +use simulator::Simulator; + +use process_calibration::process_calibration; +use std::time::{Duration, Instant}; +use std::thread; + + +fn main() { + println!("Starting calibration simulation..."); + let mut simulator = Simulator::new(); + let mut samples = Vec::new(); + let start = Instant::now(); + + while start.elapsed() < Duration::from_secs(10) { + let t = start.elapsed().as_secs_f32(); + simulator.x_pos = 50.0 + 20.0 * f32::sin(t * 2.0); + + let reading = simulator.simulate_sensor_position(); + samples.push(reading.clone()); + println!("Sample: {:?}", reading); + + thread::sleep(Duration::from_millis(500)); + } + + match process_calibration(&samples, simulator::NUM_SENSORS) { + Ok((min_values, max_values, thresholds)) => { + println!("\nCalibration results:"); + println!("Min values: {:?}", min_values); + println!("Max values: {:?}", max_values); + println!("Thresholds: {:?}", thresholds); + }, + Err(e) => { + println!("Calibration failed: {:?}", e); + } + } +} \ No newline at end of file diff --git a/apps/line_sensor_simulator/src/simulator/mod.rs b/apps/line_sensor_simulator/src/simulator/mod.rs new file mode 100644 index 0000000..ada7b2d --- /dev/null +++ b/apps/line_sensor_simulator/src/simulator/mod.rs @@ -0,0 +1,79 @@ +use std::time::SystemTime; +use rand::Rng; + +// Constants for simulation +pub const NUM_SENSORS: usize = 8; +pub const WHITE_VALUE: i32 = 2800; +pub const BLACK_VALUE: i32 = 200; +pub const TRANSITION_VALUE: i32 = 1500; +pub const SAMPLE_TIME_MS: u64 = 500; + +/// Represents the line sensor simulator +pub struct Simulator { + /// Current X position of robot + pub x_pos: f32, + /// Position of the line center + pub line_center: f32, + /// Space between sensors in units + sensor_spacing: f32, +} + +impl Simulator { + /// Creates a new simulator with default values + pub fn new() -> Self { + Self { + x_pos: 50.0, + line_center: 50.0, + sensor_spacing: 2.0, + } + } + + /// Adds random noise to sensor readings + fn add_noise(value: i32) -> i32 { + let noise = rand::thread_rng().gen_range(-50..=50); + (value + noise).clamp(0, 3000) + } + + /// Simulates readings from all sensors + pub fn simulate_sensor_position(&self) -> Vec { + let mut readings = Vec::with_capacity(NUM_SENSORS); + + for i in 0..NUM_SENSORS { + // Calculate position for each sensor + let sensor_pos = self.x_pos + (i as f32 - NUM_SENSORS as f32/2.0) * self.sensor_spacing; + let distance = (sensor_pos - self.line_center).abs(); + + // Determine sensor value based on distance from line + let raw_value = if distance < 2.0 { + BLACK_VALUE + } else if distance < 4.0 { + TRANSITION_VALUE + } else { + WHITE_VALUE + }; + + readings.push(Self::add_noise(raw_value)); + } + + readings + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_simulator_new() { + let sim = Simulator::new(); + assert_eq!(sim.x_pos, 50.0); + assert_eq!(sim.line_center, 50.0); + } + + #[test] + fn test_sensor_readings() { + let sim = Simulator::new(); + let readings = sim.simulate_sensor_position(); + assert_eq!(readings.len(), NUM_SENSORS); + } +} \ No newline at end of file diff --git a/libs/process_calibration/Cargo.toml b/libs/process_calibration/Cargo.toml new file mode 100644 index 0000000..57d209a --- /dev/null +++ b/libs/process_calibration/Cargo.toml @@ -0,0 +1,6 @@ +[package] +name = "process_calibration" +version = "0.1.0" +edition = "2021" + +[dependencies] diff --git a/libs/process_calibration/src/lib.rs b/libs/process_calibration/src/lib.rs new file mode 100644 index 0000000..24e41a3 --- /dev/null +++ b/libs/process_calibration/src/lib.rs @@ -0,0 +1,73 @@ +#[derive(Debug)] +pub enum CalibrationError { + InvalidSensorCount, + EmptySamples, + InconsistentSamples, +} + +pub fn process_calibration(samples: &[Vec], num_sensors: usize) + -> Result<(Vec, Vec, Vec), CalibrationError> { + + if samples.is_empty() { + return Err(CalibrationError::EmptySamples); + } + + if samples.iter().any(|s| s.len() != num_sensors) { + return Err(CalibrationError::InconsistentSamples); + } + + let mut min_values = vec![i32::MAX; num_sensors]; + let mut max_values = vec![i32::MIN; num_sensors]; + + for sample in samples { + for (i, &value) in sample.iter().enumerate() { + min_values[i] = min_values[i].min(value); + max_values[i] = max_values[i].max(value); + } + } + + let thresholds: Vec = min_values.iter() + .zip(max_values.iter()) + .map(|(&min, &max)| (min + max) / 2) + .collect(); + + Ok((min_values, max_values, thresholds)) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn test_valid_calibration() { + let samples = vec![ + vec![100, 200], + vec![150, 250], + ]; + let result = process_calibration(&samples, 2).unwrap(); + assert_eq!(result.0, vec![100, 200]); // min + assert_eq!(result.1, vec![150, 250]); // max + assert_eq!(result.2, vec![125, 225]); // thresholds + } + + #[test] + fn test_empty_samples() { + let samples: Vec> = vec![]; + assert!(matches!( + process_calibration(&samples, 2), + Err(CalibrationError::EmptySamples) + )); + } + + #[test] + fn test_inconsistent_samples() { + let samples = vec![ + vec![100, 200], + vec![150], + ]; + assert!(matches!( + process_calibration(&samples, 2), + Err(CalibrationError::InconsistentSamples) + )); + } +} \ No newline at end of file