From b9e5a6abd7a3888ba9d3a9ba6c62e3ef3b0da31c Mon Sep 17 00:00:00 2001 From: Flima Desktop Date: Mon, 15 Dec 2025 00:08:10 +0000 Subject: [PATCH] sim working, need to improve graphs and data analysis --- analyze.py | 73 +++++++++++++++-------------- src/main.rs | 114 ++++++++++++++++++++++++++-------------------- src/simulation.rs | 12 ++--- 3 files changed, 106 insertions(+), 93 deletions(-) diff --git a/analyze.py b/analyze.py index 409b066..6124dd9 100644 --- a/analyze.py +++ b/analyze.py @@ -1,57 +1,56 @@ import pandas as pd import matplotlib.pyplot as plt +from pathlib import Path -# Load the CSV exported by the PID controller -df = pd.read_csv("data.csv") +# -------- File picker -------- +RESULTS_DIR = Path("results") -# --- Plot target vs current --- +csv_files = sorted(RESULTS_DIR.glob("*.csv")) + +if not csv_files: + raise FileNotFoundError("No CSV files found in results/") + +print("Available result files:") +for i, f in enumerate(csv_files): + print(f"[{i}] {f.name}") + +choice = int(input("Select file number: ")) +csv_path = csv_files[choice] + +print(f"\nLoading: {csv_path}\n") + +# -------- Load CSV -------- +df = pd.read_csv(csv_path) + +# -------- Plot target vs current -------- for coord in ["x", "y", "z"]: plt.figure() - plt.plot(df["time"], df["target_" + coord], label="target_" + coord) - plt.plot(df["time"], df["current_" + coord], label="current_" + coord) - plt.plot(df["time"], df["mot_" + coord].clip(-1,1), label="mot_" + coord) - plt.plot(df["time"], df["dmot_" + coord].clip(-1,1), label="desired mot_" + coord) + plt.plot(df["time"], df[f"target_{coord}"], label=f"target_{coord}") + plt.plot(df["time"], df[f"current_{coord}"], label=f"current_{coord}") + plt.plot(df["time"], df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}") + plt.plot(df["time"], df[f"dmot_{coord}"].clip(-1, 1), label=f"desired mot_{coord}") + plt.xlabel("Time (s)") plt.ylabel("Angular velocity & Motor Offset") - plt.title(f"Target vs Current Angular Velocity with Motor Diff ({coord})") - plt.legend(loc='upper right') + plt.title(f"{csv_path.name} — {coord.upper()} axis") + plt.legend(loc="upper right") plt.grid(True) + + ymin, ymax = plt.ylim() + plt.ylim(min(ymin, -1), max(ymax, 1)) + plt.show() -# for coord in ["x", "y", "z"]: -# plt.figure() -# plt.plot(df["time"], df["target_" + coord], label="target_" + coord) -# plt.plot(df["time"], df["current_" + coord], label="current_" + coord) -# plt.xlabel("Time (s)") -# plt.ylabel("Angular velocity") -# plt.title(f"Target vs Current Angular Velocity ({coord})") -# plt.legend() -# plt.grid(True) -# plt.show() - - -# plt.figure() -# plt.plot(df["time"], df["target_x"], label="target_x") -# plt.plot(df["time"], df["current_x"], label="current_x") -# plt.plot(df["time"], df["target_y"], label="target_y") -# plt.plot(df["time"], df["current_y"], label="current_y") -# plt.plot(df["time"], df["target_z"], label="target_z") -# plt.plot(df["time"], df["current_z"], label="current_z") -# plt.xlabel("Time (s)") -# plt.ylabel("Angular velocity") -# plt.title("Target vs Current Angular Velocity") -# plt.legend() -# plt.grid(True) -# plt.show() -# --- Plot error over time --- +# -------- Plot error -------- plt.figure() plt.plot(df["time"], df["error_x"], label="error_x") plt.plot(df["time"], df["error_y"], label="error_y") plt.plot(df["time"], df["error_z"], label="error_z") + plt.xlabel("Time (s)") plt.ylabel("Error") -plt.title("PID Error Over Time") -plt.legend(loc='upper right') +plt.title(f"PID Error — {csv_path.name}") +plt.legend(loc="upper right") plt.grid(True) plt.show() diff --git a/src/main.rs b/src/main.rs index 420a208..2fd2c54 100644 --- a/src/main.rs +++ b/src/main.rs @@ -34,6 +34,8 @@ fn window_conf() -> mq::Conf { } use std::env; +use std::path::PathBuf; +use std::thread::JoinHandle; enum RunMode { Batch, @@ -62,68 +64,80 @@ async fn main() { } async fn run_batch() { + fs::remove_dir_all(RESULTS_DIR).unwrap(); fs::create_dir_all(RESULTS_DIR).unwrap(); let input_files = list_files(INPUTS_DIR); let config_files = list_files(CONFIGS_DIR); + let mut handles: Vec> = Default::default(); + for input_path in input_files { - let input_name = input_path.file_stem().unwrap().to_string_lossy(); - - let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap()) - .expect("Failed to load input recording"); - for config_path in &config_files { - let config_name = config_path.file_stem().unwrap().to_string_lossy(); - - let config: SimulationConfig = - toml::from_str(&fs::read_to_string(config_path).unwrap()) - .expect("Invalid config file"); - - println!( - "Running simulation: input={} config={}", - input_name, config_name - ); - - /* World */ - let mut world = World::new(config.tickrate); - - /* Drone */ - let drone = drone::Drone::new( - &mut world, - Box::new(drone::pidcontroller::PIDController { - target_rate: config.target_rate, - proportional_multiplier: config.proportional(), - integral_multiplier: config.integral(), - diferential_multiplier: config.diferential(), - ..Default::default() - }), - drone::MotorCharacteristics { - max_thrust: config.max_thrust, - max_torque: config.max_torque, - ..Default::default() - }, - config.mass, - ); - - let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name); - - let mut sim = Simulation::new( - drone, - world, - false, - SimMode::Playback(inputs.clone(), 0.0), - Some(result_file), - config.drone_tick_rate, - ); - - sim.run().await.unwrap(); + let cp = config_path.clone(); + let ip = input_path.clone(); + handles.push(std::thread::spawn(move || { + run(&ip, &cp); + })); } } + for handle in handles { + handle.join().unwrap(); + } println!("All simulations completed."); } +fn run(input_path: &PathBuf, config_path: &PathBuf) { + let input_name = input_path.file_stem().unwrap().to_string_lossy(); + + let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap()) + .expect("Failed to load input recording"); + let config_name = config_path.file_stem().unwrap().to_string_lossy(); + + let config: SimulationConfig = + toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file"); + + println!( + "Running simulation: input={} config={}", + input_name, config_name + ); + + /* World */ + let mut world = World::new(config.tickrate); + + /* Drone */ + let drone = drone::Drone::new( + &mut world, + Box::new(drone::pidcontroller::PIDController { + target_rate: config.target_rate, + proportional_multiplier: config.proportional(), + integral_multiplier: config.integral(), + diferential_multiplier: config.diferential(), + ..Default::default() + }), + drone::MotorCharacteristics { + max_thrust: config.max_thrust, + max_torque: config.max_torque, + ..Default::default() + }, + config.mass, + ); + + let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name); + + let mut sim = Simulation::new( + drone, + world, + false, + SimMode::Playback(inputs.clone(), 0.0), + Some(result_file), + config.drone_tick_rate, + ); + + sim.run().unwrap(); +} + async fn run_record(output: String) { println!("Recording inputs to {}", output); @@ -152,5 +166,5 @@ async fn run_record(output: String) { 600, ); - sim.run().await.unwrap(); + sim.run().unwrap(); } diff --git a/src/simulation.rs b/src/simulation.rs index fc3c8e2..a25f8d7 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -89,7 +89,7 @@ impl Simulation { return s; } - pub async fn run(&mut self) -> Result<(), Box> { + pub fn run(&mut self) -> Result<(), Box> { let mut current_input: JoystickInput; loop { @@ -97,16 +97,16 @@ impl Simulation { renderer.update_camera(&self.world); } - if mq::is_key_pressed(mq::KeyCode::Q) { - break; - } - // --- Input handling --- let current_time = self.world.get_time() as f32; match &mut self.mode { SimMode::Record(recording, _) => { current_input = recording.add_input_from_keyboard(current_time); + if mq::is_key_pressed(mq::KeyCode::Q) { + self.shutdown()?; + return Ok(()); + } } SimMode::Playback(recording, start_time) => { let playback_time = current_time - *start_time; @@ -187,7 +187,7 @@ impl Simulation { == 0 { renderer.draw(&mut self.world); - mq::next_frame().await; + mq::next_frame(); } } }