graphing data. PID controller kinda? No params yet

analyze.py

@@ -0,0 +1,43 @@
+import pandas as pd
+import matplotlib.pyplot as plt
+
+# Load the CSV exported by the PID controller
+df = pd.read_csv("pid_logs.csv")
+
+# --- 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.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 ---
+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()
+plt.grid(True)
+plt.show()
+

flake.nix

@@ -31,6 +31,12 @@
 
               glfw
               cmake
+              (pkgs.python3.withPackages (python-pkgs:
+                with python-pkgs; [
+                  # select Python packages here
+                  pandas
+                  matplotlib
+                ]))
             ];
 
             shellHook = ''

src/drone.rs

@@ -136,7 +136,7 @@ impl Drone {
             .iter()
             .enumerate()
         {
-            let throttle = throttles[i].clamp(-1.0, 1.0);
+            let throttle = throttles[i].clamp(0.0, 1.0);
             let thrust = self.motor_characteristics.max_thrust * throttle;
             let torque = self.motor_characteristics.max_torque * throttle;
 

src/drone/pidcontroller.rs

@@ -1,45 +1,85 @@
-use nalgebra::{self as na, Vector3};
-use rapier3d::prelude as rp;
-use std::{any::Any, f32, io::PipeReader};
+use nalgebra::{self as na, vector};
+use std::{any::Any, f32};
 
 use crate::drone::controller::DroneController;
 pub use crate::drone::controller::JoystickInput;
 use crate::drone::MotorCharacteristics;
 
+const PROPORTIONAL_ONLY: bool = false;
+
 #[derive(Default)]
-pub struct PControllerState {
+pub struct PIDControllerState {
     last_time: f32,
     current_time: f32,
     last_rotation: na::Unit<na::Quaternion<f32>>,
     current_rotation: na::Unit<na::Quaternion<f32>>,
     angular_velocity: na::Vector3<f32>,
+    last_error: na::Vector3<f32>,
+    error_sum: na::Vector3<f32>,
+
+    pub log_time: Vec<f32>,
+    pub log_target: Vec<na::Vector3<f32>>,
+    pub log_current: Vec<na::Vector3<f32>>,
+    pub log_error: Vec<na::Vector3<f32>>,
 }
 
-impl PControllerState {
+impl PIDControllerState {
     pub fn dt(&self) -> f32 {
         return self.current_time - self.last_time;
     }
 }
 
-pub struct PController {
+pub struct PIDController {
     input: JoystickInput,
     pub target_rate: f32,
+    pub proportional_multiplier: na::Vector3<f32>,
+    pub integral_multiplier: na::Vector3<f32>,
+    pub diferential_multiplier: na::Vector3<f32>,
     pub multiply_mode: bool,
-    state: PControllerState,
+    pub state: PIDControllerState,
 }
 
-impl Default for PController {
+impl Default for PIDController {
     fn default() -> Self {
         Self {
             input: JoystickInput::default(),
             target_rate: f32::consts::PI,
             state: Default::default(),
             multiply_mode: false,
+            proportional_multiplier: vector![0.13, 3.0, 0.13],
+            integral_multiplier: vector![0.0, 0.0, 0.0],
+            diferential_multiplier: vector![0.001, 0.01, 0.001],
         }
     }
 }
 
-impl PController {
+impl PIDController {
+    /// Save log buffers to a CSV file
+    pub fn save_logs_to_csv(&self, path: &str) -> std::io::Result<()> {
+        use std::fs::File;
+        use std::io::Write;
+
+        let mut file = File::create(path)?;
+        writeln!(
+            file,
+            "time,target_x,target_y,target_z,current_x,current_y,current_z,error_x,error_y,error_z"
+        )?;
+
+        for i in 0..self.state.log_time.len() {
+            let t = self.state.log_time[i];
+            let tg = self.state.log_target[i];
+            let cur = self.state.log_current[i];
+            let err = self.state.log_error[i];
+            writeln!(
+                file,
+                "{},{},{},{},{},{},{},{},{},{}",
+                t, tg.x, tg.y, tg.z, cur.x, cur.y, cur.z, err.x, err.y, err.z
+            )?;
+        }
+
+        Ok(())
+    }
+
     pub fn set_input(&mut self, input: JoystickInput) {
         self.input = input;
     }
@@ -57,7 +97,7 @@ impl PController {
     }
 }
 
-impl DroneController for PController {
+impl DroneController for PIDController {
     fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
         self.state.last_rotation = self.state.current_rotation;
         self.state.current_rotation = rotation;
@@ -75,12 +115,44 @@ impl DroneController for PController {
         let current = rot.inverse().transform_vector(&self.get_angular_velocity());
         let target = self.get_desired_angular_velocity();
 
-        let diff = target - current;
+        let error: na::Vector3<f32> = target - current;
         let throttle = self.input.throttle_input;
 
-        let roll = diff.z * 0.01;
-        let pitch = diff.x * 0.01;
-        let yaw = diff.y;
+        self.state.log_time.push(self.state.current_time);
+        self.state.log_target.push(target);
+        self.state.log_current.push(current);
+        self.state.log_error.push(error);
+
+        let error_dif: na::Vector3<f32> = error - self.state.last_error;
+
+        let forces_to_apply = match PROPORTIONAL_ONLY {
+            true => {
+                vector![
+                    error.x * self.proportional_multiplier.x,
+                    error.y * self.proportional_multiplier.y,
+                    error.z * self.proportional_multiplier.z
+                ]
+            }
+            false => {
+                vector![
+                    error.x * self.proportional_multiplier.x
+                        + self.state.error_sum.x * self.integral_multiplier.x
+                        + error_dif.x * self.diferential_multiplier.x,
+                    error.y * self.proportional_multiplier.y
+                        + self.state.error_sum.y * self.integral_multiplier.y
+                        + error_dif.y * self.diferential_multiplier.y,
+                    error.z * self.proportional_multiplier.z
+                        + self.state.error_sum.z * self.integral_multiplier.z
+                        + error_dif.z * self.diferential_multiplier.z,
+                ]
+            }
+        };
+        self.state.error_sum += error;
+        self.state.last_error = error;
+
+        let roll = forces_to_apply.z;
+        let pitch = forces_to_apply.x;
+        let yaw = forces_to_apply.y;
 
         let mut motors: [f32; 4] = match self.multiply_mode {
             true => {
@@ -96,7 +168,7 @@ impl DroneController for PController {
                 t
             }
             false => {
-                let mut t = [
+                let t = [
                     throttle + yaw - pitch + roll,
                     throttle - yaw - pitch - roll,
                     throttle + yaw + pitch - roll,

src/engine.rs

@@ -122,7 +122,7 @@ impl Default for World {
     fn default() -> Self {
         let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
         let mut integration_parameters = rp::IntegrationParameters::default();
-        integration_parameters.set_inv_dt(60.0);
+        integration_parameters.set_inv_dt(600.0);
         let physics_pipeline = rp::PhysicsPipeline::new();
         let island_manager = rp::IslandManager::new();
         let broad_phase = rp::DefaultBroadPhase::new();

src/main.rs

@@ -40,7 +40,7 @@ async fn main() {
         None,
     );
 
-    let mut drone_controller = drone::pidcontroller::PController::default();
+    let mut drone_controller = drone::pidcontroller::PIDController::default();
     drone_controller.set_input(JoystickInput {
         throttle_input: 0.75,
         yaw_input: 0.4,
@@ -91,6 +91,9 @@ async fn main() {
             renderer.update_light(&world);
         }
 
+        if mq::is_key_pressed(mq::KeyCode::Q) {
+            break;
+        }
         if mq::is_key_pressed(mq::KeyCode::C) {
             add_objects(&mut world);
         }
@@ -132,7 +135,7 @@ async fn main() {
         match drone_obj
             .controller
             .as_mut_any()
-            .downcast_mut::<drone::pidcontroller::PController>()
+            .downcast_mut::<drone::pidcontroller::PIDController>()
         {
             Some(cont) => {
                 cont.set_input(input);
@@ -148,14 +151,25 @@ async fn main() {
         // Rendering
         renderer.draw(&mut world);
 
-        if world.tick % (60 / 30) == 0 {
-            renderer.update_light(&world);
-            world.clear_ofb();
-        }
+        if world.tick % (20) == 0 {
+            // renderer.update_light(&world);
+            // world.clear_ofb();
             mq::next_frame().await;
         }
     }
 
+    match drone_obj
+        .controller
+        .as_mut_any()
+        .downcast_mut::<drone::pidcontroller::PIDController>()
+    {
+        Some(cont) => {
+            let _ = cont.save_logs_to_csv("./pid_logs.csv");
+        }
+        None => {}
+    };
+}
+
 fn add_objects(world: &mut World) {
     for i in 0..1 {
         let body = world.register_body(