before cargo fix

Cargo.toml

@@ -17,11 +17,13 @@ serde = {version = "1.0.228", features = ["serde_derive"]}
 toml = "0.9.8"
 
 
-
 [[bin]]
-name = "batch"
+name = "test"
-path = "src/main_batch.rs"
+path = "src/main_test.rs"
 
 
-[profile.dev.package.rapier3d]
+# [profile.dev.package.rapier3d]
+# opt-level = 3
+
+[profile.dev.package."*"]
 opt-level = 3

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.ax.plot(
-            self.df["time"],
+        #     self.df["time"],
-            self.df[f"dmot_{coord}"].clip(-1, 1),
+        #     self.df[f"dmot_{coord}"].clip(-1, 1),
-            label=f"desired mot_{coord}",
+        #     label=f"desired mot_{coord}",
-            linestyle="--",
+        #     linestyle="--",
-        )
+        # )
 
         self.ax.set_xlabel("Time (s)")
         self.ax.set_ylabel("Value")

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<LayerConfig>,
 
-    /// 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

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;

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<f32>,
     pub angular_velocity: na::Vector3<f32>,
 }
 
-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<f32>,
-        state: &DroneState,
-        dt: f32,
-        is_first_layer: bool,
-    ) -> na::Vector3<f32> {
-        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<ControllerModule>,
     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 ---
+        return self.mixer.mix(self.input.throttle_input, setpoint).0;
-        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
     }
 
     fn as_any(&self) -> &dyn Any {
@@ -183,39 +110,3 @@ impl DroneController for StackedController {
         self
     }
 }
-
-pub struct PidProcessor {
-    kp: na::Vector3<f32>,
-    ki: na::Vector3<f32>,
-    kd: na::Vector3<f32>,
-    integral: na::Vector3<f32>,
-    last_error: na::Vector3<f32>,
-}
-
-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<f32>,
-        current: na::Vector3<f32>,
-        dt: f32,
-    ) -> na::Vector3<f32> {
-        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)
-    }
-}

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 {
+use crate::config::SimulationConfig;
-    mq::Conf {
+use helpers::list_files;
-        window_title: "RustDroneSim".to_owned(),
+use std::fs;
-        window_resizable: true,
+
-        // fullscreen: true,
+const INPUTS_DIR: &str = "inputs/";
-        platform: mq::miniquad::conf::Platform {
+const CONFIGS_DIR: &str = "configurations/";
-            // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
+const RESULTS_DIR: &str = "results/";
-            ..Default::default()
+
-        },
+use std::env;
-        ..Default::default()
+use std::path::PathBuf;
-    }
+use std::thread::JoinHandle;
+
+fn main() {
+    run_batch();
 }
 
-#[macroquad::main(window_conf)]
+fn run_batch() {
-async fn main() {
+    let _ = fs::remove_dir_all(RESULTS_DIR);
-    /* World Setup */
+    let _ = fs::create_dir_all(RESULTS_DIR);
-    let mut world = World::new(600.0);
 
-    world.register_free_collider(
+    let input_files = list_files(INPUTS_DIR);
-        ColliderBuilder::cuboid(30.0, 1.0, 30.0)
+    let config_files = list_files(CONFIGS_DIR);
-            .translation(vector![0.0, -2.0, 0.0])
+
-            .restitution(0.5)
+    let mut handles: Vec<JoinHandle<()>> = Default::default();
-            .build(),
+
-        None,
+    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();
+    let mut world = World::new(config.tickrate);
-    drone_controller.set_input(JoystickInput {
+
-        throttle_input: 0.2,
+    let drone = drone::Drone::new(
-        yaw_input: 0.2,
-        roll_input: 0.2,
-        pitch_input: 0.0,
-    });
-    let mut drone_obj = 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 mut sim = Simulation::new(
-    let camera = camera::AttachedCamera::new(
+        drone,
-        drone_obj.rb_handle,
+        world,
-        vector![1.0, 0.0, 0.0],
+        SimMode::Record(InputRecording::default(), output),
-        vector![0.5, 0.0, 0.0],
+        None,
+        600,
     );
-    let mut renderer = Renderer::new(Box::new(camera));
 
-    renderer.light.set_location(
+    sim.run_and_render().await.unwrap();
-        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;
-        }
-    }
 }

src/main_batch.rs

@@ -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<JoinHandle<()>> = 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();
-}