From 13f87452ecf62eace979df48a1bd064cdbd23675 Mon Sep 17 00:00:00 2001 From: franchioping Date: Thu, 5 Feb 2026 20:26:30 +0000 Subject: [PATCH] before cargo fix --- Cargo.toml | 16 ++-- analyze.py | 12 +-- src/config.rs | 2 +- src/drone.rs | 2 +- src/drone/stacked.rs | 135 ++++----------------------------- src/main.rs | 173 +++++++++++++++++++++++++------------------ src/main_batch.rs | 126 ------------------------------- 7 files changed, 132 insertions(+), 334 deletions(-) delete mode 100644 src/main_batch.rs diff --git a/Cargo.toml b/Cargo.toml index 8c7c106..9dec5c9 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -6,22 +6,24 @@ default-run = "RustPhysicsMQ" [dependencies] macroquad = "0.4.14" -rapier3d = { version = "0.31", features = [ "simd-stable" ] } -nalgebra = {version="0.34", features = ["convert-glam027"]} +rapier3d = { version = "0.31", features = ["simd-stable"] } +nalgebra = { version = "0.34", features = ["convert-glam027"] } glam = "0.27" rand = "0.9.2" strum = { version = "0.27", features = ["derive"] } clearscreen = "4.0.2" gilrs = "0.11.0" -serde = {version = "1.0.228", features = ["serde_derive"]} +serde = { version = "1.0.228", features = ["serde_derive"] } toml = "0.9.8" - [[bin]] -name = "batch" -path = "src/main_batch.rs" +name = "test" +path = "src/main_test.rs" -[profile.dev.package.rapier3d] +# [profile.dev.package.rapier3d] +# opt-level = 3 + +[profile.dev.package."*"] opt-level = 3 diff --git a/analyze.py b/analyze.py index 537cfcf..38c9d62 100644 --- a/analyze.py +++ b/analyze.py @@ -124,12 +124,12 @@ class SimVisualizer: self.ax.plot( self.df["time"], self.df[f"mot_{coord}"].clip(-1, 1), label=f"mot_{coord}" ) - self.ax.plot( - self.df["time"], - self.df[f"dmot_{coord}"].clip(-1, 1), - label=f"desired mot_{coord}", - linestyle="--", - ) + # self.ax.plot( + # self.df["time"], + # self.df[f"dmot_{coord}"].clip(-1, 1), + # label=f"desired mot_{coord}", + # linestyle="--", + # ) self.ax.set_xlabel("Time (s)") self.ax.set_ylabel("Value") diff --git a/src/config.rs b/src/config.rs index 868568d..767515b 100644 --- a/src/config.rs +++ b/src/config.rs @@ -34,7 +34,7 @@ pub struct SimulationConfig { // The order of this Vec defines the stack (e.g., [Angle, Rate]) pub layers: Vec, - /// Maps [Pitch, Yaw, Roll] to each of the 4 motors + /// Maps [Roll, Yaw, Pitch] to each of the 4 motors pub motor_map: [[f32; 3]; 4], // Motors & Physics diff --git a/src/drone.rs b/src/drone.rs index 8c7a645..03c3150 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -82,7 +82,7 @@ impl Drone { */ // .linear_damping(0.2) // Damps velocity slowly .angular_damping(0.1) // Damps angular velocity slowly - .gyroscopic_forces_enabled(true) + .gyroscopic_forces_enabled(false) .build(), ); let width = 0.40; diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs index 9d4949e..82a1a78 100644 --- a/src/drone/stacked.rs +++ b/src/drone/stacked.rs @@ -8,71 +8,21 @@ use crate::drone::JoystickInput; use crate::config::*; +pub mod mixer; +pub mod modules; + +use mixer::MotorMixer; +use modules::{ControllerModule, PidProcessor}; + pub struct DroneState { pub rotation: na::UnitQuaternion, pub angular_velocity: na::Vector3, } -pub enum ControllerModule { - Rate { - processor: PidProcessor, - max_rate: f32, - }, - Angle { - processor: PidProcessor, - max_angle: f32, - }, -} - -impl ControllerModule { - /// Takes the current setpoint (from joystick or previous layer) and - /// returns the command for the next layer. - pub fn process( - &mut self, - setpoint: na::Vector3, - state: &DroneState, - dt: f32, - is_first_layer: bool, - ) -> na::Vector3 { - match self { - ControllerModule::Angle { - processor, - max_angle, - } => { - // Setpoint is -1.0..1.0, scale it to target Radians - let target_angles = setpoint * *max_angle; - - let (r, p, y) = state.rotation.euler_angles(); - let current_angles = na::vector![r, y, p]; - - // Output of Angle PID = Desired Angular Velocity - processor.update(target_angles, current_angles, dt) - } - - ControllerModule::Rate { - processor, - max_rate, - } => { - // If Rate is the start of the chain (Acro mode), scale the joystick. - // If it's the second layer, the setpoint is already a velocity from the Angle layer. - let target_velocity = if is_first_layer { - setpoint * *max_rate - } else { - setpoint - }; - let rot = state.rotation; - let current = rot.inverse().transform_vector(&state.angular_velocity); - - // Output of Rate PID = Desired Torque/Correction Force - processor.update(target_velocity, current, dt) - } - } - } -} - pub struct StackedController { modules: Vec, config: SimulationConfig, + mixer: MotorMixer, // State drone_state: DroneState, @@ -106,6 +56,11 @@ impl StackedController { } Self { + mixer: MotorMixer { + motor_map: config.motor_map, + min_throttle: 0.0, + max_throttle: 1.0, + }, modules, config, input: JoystickInput::default(), @@ -134,46 +89,18 @@ impl DroneController for StackedController { fn get_motor_throttles(&mut self) -> [f32; 4] { let dt = (self.current_time - self.last_time).max(0.0001); - // Input normalized -1.0 to 1.0 from joystick let mut setpoint = vector![ self.input.roll_input, self.input.yaw_input, self.input.pitch_input, ]; - // --- CASCADED PROCESSING --- - // If Layer 0 is Angle, setpoint becomes Desired Rate. - // If Layer 1 is Rate, it takes that Desired Rate and outputs Torque. - // - for (i, module) in self.modules.iter_mut().enumerate() { let is_first_layer = i == 0; setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer); } - // --- MOTOR MIXER --- - let mut motors = [0.0; 4]; - for i in 0..4 { - let map = self.config.motor_map[i]; - motors[i] = self.input.throttle_input - + (map[0] * setpoint.x) // Roll - + (map[1] * setpoint.y) // Yaw - + (map[2] * setpoint.z); // Pitch - } - - // Clamp while keeping relative values - let max = motors - .iter() - .copied() - .max_by(|a, b| a.total_cmp(b)) - .unwrap_or(0.0); - if max > 1.0 { - for v in motors.iter_mut() { - *v /= max; - } - } - - motors + return self.mixer.mix(self.input.throttle_input, setpoint).0; } fn as_any(&self) -> &dyn Any { @@ -183,39 +110,3 @@ impl DroneController for StackedController { self } } - -pub struct PidProcessor { - kp: na::Vector3, - ki: na::Vector3, - kd: na::Vector3, - integral: na::Vector3, - last_error: na::Vector3, -} - -impl PidProcessor { - fn new(c: &PidConfig) -> Self { - Self { - kp: c.p_vec(), - ki: c.i_vec(), - kd: c.d_vec(), - integral: na::Vector3::zeros(), - last_error: na::Vector3::zeros(), - } - } - - fn update( - &mut self, - target: na::Vector3, - current: na::Vector3, - dt: f32, - ) -> na::Vector3 { - let error = target - current; - self.integral += error * dt; - let derivative = (error - self.last_error) / dt; - self.last_error = error; - - error.component_mul(&self.kp) - + self.integral.component_mul(&self.ki) - + derivative.component_mul(&self.kd) - } -} diff --git a/src/main.rs b/src/main.rs index 06bb4ec..6e3352b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,54 +1,111 @@ -use macroquad::prelude as mq; -use rapier3d::prelude::*; - mod engine; use engine::*; - mod camera; -mod drone; -mod rendering; - mod config; +mod drone; +mod helpers; +mod rendering; +mod simulation; + mod graphics_util; -use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer}; +use crate::drone::input::*; +use crate::simulation::{SimMode, Simulation}; -fn window_conf() -> mq::Conf { - mq::Conf { - window_title: "RustDroneSim".to_owned(), - window_resizable: true, - // fullscreen: true, - platform: mq::miniquad::conf::Platform { - // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly, - ..Default::default() - }, - ..Default::default() - } +use crate::config::SimulationConfig; +use helpers::list_files; +use std::fs; + +const INPUTS_DIR: &str = "inputs/"; +const CONFIGS_DIR: &str = "configurations/"; +const RESULTS_DIR: &str = "results/"; + +use std::env; +use std::path::PathBuf; +use std::thread::JoinHandle; + +fn main() { + run_batch(); } -#[macroquad::main(window_conf)] -async fn main() { - /* World Setup */ - let mut world = World::new(600.0); +fn run_batch() { + let _ = fs::remove_dir_all(RESULTS_DIR); + let _ = fs::create_dir_all(RESULTS_DIR); - world.register_free_collider( - ColliderBuilder::cuboid(30.0, 1.0, 30.0) - .translation(vector![0.0, -2.0, 0.0]) - .restitution(0.5) - .build(), - None, + 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 { + for config_path in &config_files { + 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 ); - let mut drone_controller = drone::pidcontroller::PIDController::default(); - drone_controller.set_input(JoystickInput { - throttle_input: 0.2, - yaw_input: 0.2, - roll_input: 0.2, - pitch_input: 0.0, - }); - let mut drone_obj = drone::Drone::new( + let mut world = World::new(config.tickrate); + + let drone = drone::Drone::new( &mut world, - Box::new(drone_controller), + Box::new(drone::stacked::StackedController::new(config.clone())), + drone::MotorCharacteristics { + max_thrust: config.max_thrust, + max_torque: config.max_torque, + time_constant: config.time_constant, + ..Default::default() + }, + config.mass, + ); + + let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name); + + let mut sim = Simulation::new( + drone, + world, + 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); + + let tickrate = 60000.0; + let mut world = World::new(tickrate); + + let drone = drone::Drone::new( + &mut world, + Box::new(drone::pidcontroller::PIDController { + ..Default::default() + }), drone::MotorCharacteristics { max_thrust: 2.6, max_torque: 0.5, @@ -57,39 +114,13 @@ async fn main() { 0.350, ); - /* Renderer Setup */ - let camera = camera::AttachedCamera::new( - drone_obj.rb_handle, - vector![1.0, 0.0, 0.0], - vector![0.5, 0.0, 0.0], + let mut sim = Simulation::new( + drone, + world, + SimMode::Record(InputRecording::default(), output), + None, + 600, ); - let mut renderer = Renderer::new(Box::new(camera)); - renderer.light.set_location( - vector![70.0, 150.0, -90.0].into(), - vector![-0.4, -0.7, 0.6].into(), - ); - renderer.update_light(&world); - - loop { - renderer.update_camera(&world); - - if mq::is_key_pressed(mq::KeyCode::Q) { - break; - } - - // Physics - world.step(); - let _ = clearscreen::clear(); - drone_obj.process_tick(&mut world); - - // Rendering - renderer.draw(&mut world); - - if world.tick % (10) == 0 { - // renderer.update_light(&world); - // world.clear_ofb(); - mq::next_frame().await; - } - } + sim.run_and_render().await.unwrap(); } diff --git a/src/main_batch.rs b/src/main_batch.rs deleted file mode 100644 index 6e3352b..0000000 --- a/src/main_batch.rs +++ /dev/null @@ -1,126 +0,0 @@ -mod engine; -use engine::*; -mod camera; -mod config; -mod drone; -mod helpers; -mod rendering; -mod simulation; - -mod graphics_util; - -use crate::drone::input::*; -use crate::simulation::{SimMode, Simulation}; - -use crate::config::SimulationConfig; -use helpers::list_files; -use std::fs; - -const INPUTS_DIR: &str = "inputs/"; -const CONFIGS_DIR: &str = "configurations/"; -const RESULTS_DIR: &str = "results/"; - -use std::env; -use std::path::PathBuf; -use std::thread::JoinHandle; - -fn main() { - run_batch(); -} - -fn run_batch() { - let _ = fs::remove_dir_all(RESULTS_DIR); - let _ = fs::create_dir_all(RESULTS_DIR); - - 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 { - for config_path in &config_files { - 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 - ); - - let mut world = World::new(config.tickrate); - - let drone = drone::Drone::new( - &mut world, - Box::new(drone::stacked::StackedController::new(config.clone())), - drone::MotorCharacteristics { - max_thrust: config.max_thrust, - max_torque: config.max_torque, - time_constant: config.time_constant, - ..Default::default() - }, - config.mass, - ); - - let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name); - - let mut sim = Simulation::new( - drone, - world, - 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); - - let tickrate = 60000.0; - let mut world = World::new(tickrate); - - let drone = drone::Drone::new( - &mut world, - Box::new(drone::pidcontroller::PIDController { - ..Default::default() - }), - drone::MotorCharacteristics { - max_thrust: 2.6, - max_torque: 0.5, - ..Default::default() - }, - 0.350, - ); - - let mut sim = Simulation::new( - drone, - world, - SimMode::Record(InputRecording::default(), output), - None, - 600, - ); - - sim.run_and_render().await.unwrap(); -}