WIP

analyze.py

@@ -1,56 +1,148 @@
-import pandas as pd
+import tkinter as tk
-import matplotlib.pyplot as plt
 from pathlib import Path
+from tkinter import messagebox, ttk
 
-# -------- File picker --------
+import matplotlib.pyplot as plt
-RESULTS_DIR = Path("results")
+import pandas as pd
+from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2Tk
 
-csv_files = sorted(RESULTS_DIR.glob("*.csv"))
 
-if not csv_files:
+class SimVisualizer:
-    raise FileNotFoundError("No CSV files found in results/")
+    def __init__(self, root: tk.Tk):
+        self.root = root
+        self.root.title("Simulation Data Viewer")
 
-print("Available result files:")
+        # --- Protocol Handler ---
-for i, f in enumerate(csv_files):
+        # This handles the "X" button on the window frame
-    print(f"[{i}] {f.name}")
+        self.root.protocol("WM_DELETE_WINDOW", self.quit_app)
 
-choice = int(input("Select file number: "))
+        self.results_dir = Path("results")
-csv_path = csv_files[choice]
+        self.df = None
+        self.current_file = None
 
-print(f"\nLoading: {csv_path}\n")
+        self.setup_ui()
+        self.refresh_file_list()
 
-# -------- Load CSV --------
+    def setup_ui(self):
-df = pd.read_csv(csv_path)
+        # 1. Create a PanedWindow (Horizontal orientation)
+        self.paned_window = ttk.PanedWindow(self.root, orient=tk.HORIZONTAL)
+        self.paned_window.pack(fill=tk.BOTH, expand=True)
 
-# -------- Plot target vs current --------
+        # --- Sidebar Frame ---
-for coord in ["x"]:
+        # Note: We still define a width, but it's now the *initial* width
-    plt.figure()
+        sidebar = ttk.Frame(self.paned_window, width=300, padding="10")
-    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)")
+        # 2. Add the sidebar to the PanedWindow
-    plt.ylabel("Angular velocity & Motor Offset")
+        # 'weight=0' keeps it from growing automatically when the window is resized
-    plt.title(f"{csv_path.name} — {coord.upper()} axis")
+        self.paned_window.add(sidebar, weight=0)
-    plt.legend(loc="upper right")
-    plt.grid(True)
 
-    ymin, ymax = plt.ylim()
+        ttk.Label(sidebar, text="Result Files", font=("Helvetica", 12, "bold")).pack(
-    plt.ylim(min(ymin, -1), max(ymax, 1))
+            pady=5
+        )
 
-    plt.show()
+        self.file_listbox = tk.Listbox(sidebar, height=15)
+        self.file_listbox.pack(fill=tk.X, pady=5)
+        self.file_listbox.bind("<<ListboxSelect>>", self.on_file_select)
 
-# -------- Plot error --------
+        ttk.Separator(sidebar, orient="horizontal").pack(fill="x", pady=10)
-# 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(f"PID Error — {csv_path.name}")
-# plt.legend(loc="upper right")
-# plt.grid(True)
-# plt.show()
 
+        ttk.Label(sidebar, text="Select Axis", font=("Helvetica", 10, "bold")).pack(
+            pady=5
+        )
+        self.axis_var = tk.StringVar(value="x")
+        for axis in ["x", "y", "z"]:
+            ttk.Radiobutton(
+                sidebar,
+                text=f"{axis.upper()} Axis",
+                variable=self.axis_var,
+                value=axis,
+                command=self.update_plot,
+            ).pack(anchor=tk.W)
+
+        # --- Quit Button ---
+        spacer = ttk.Label(sidebar, text="")
+        spacer.pack(fill=tk.Y, expand=True)
+
+        self.quit_button = ttk.Button(
+            sidebar, text="Quit Program", command=self.quit_app
+        )
+        self.quit_button.pack(fill=tk.X, pady=10)
+
+        # --- Plot Frame ---
+        self.plot_frame = ttk.Frame(self.paned_window, padding="10")
+
+        # 3. Add the plot frame to the PanedWindow
+        # 'weight=1' ensures the plot area expands to take up all remaining space
+        self.paned_window.add(self.plot_frame, weight=1)
+
+        self.fig, self.ax = plt.subplots(figsize=(8, 6))
+        self.canvas = FigureCanvasTkAgg(self.fig, master=self.plot_frame)
+        self.canvas.get_tk_widget().pack(fill=tk.BOTH, expand=True)
+
+        self.toolbar = NavigationToolbar2Tk(self.canvas, self.plot_frame)
+        self.toolbar.update()
+
+    def quit_app(self):
+        """Cleanly close the plots and the application."""
+        plt.close("all")
+        self.root.destroy()
+        self.root.quit()
+
+    def refresh_file_list(self):
+        if not self.results_dir.exists():
+            self.results_dir.mkdir(parents=True)
+
+        csv_files = sorted(self.results_dir.glob("*.csv"))
+        self.file_listbox.delete(0, tk.END)
+        for f in csv_files:
+            self.file_listbox.insert(tk.END, f.name)
+
+    def on_file_select(self, event):
+        selection = self.file_listbox.curselection()
+        if selection:
+            filename = self.file_listbox.get(selection[0])
+            self.current_file = self.results_dir / filename
+            try:
+                self.df = pd.read_csv(self.current_file)
+                self.update_plot()
+            except Exception as e:
+                messagebox.showerror("Error", f"Could not load file: {e}")
+
+    def update_plot(self):
+        if self.df is None:
+            return
+
+        coord = self.axis_var.get()
+        self.ax.clear()
+
+        self.ax.plot(
+            self.df["time"], self.df[f"target_{coord}"], label=f"target_{coord}"
+        )
+        self.ax.plot(
+            self.df["time"], self.df[f"current_{coord}"], label=f"current_{coord}"
+        )
+        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.set_xlabel("Time (s)")
+        self.ax.set_ylabel("Value")
+        self.ax.set_title(f"{self.current_file.name} — {coord.upper()} Axis")
+        self.ax.legend(loc="upper right")
+        self.ax.grid(True)
+
+        ymin, ymax = self.ax.get_ylim()
+        self.ax.set_ylim(min(ymin, -1.1), max(ymax, 1.1))
+        self.canvas.draw()
+
+
+if __name__ == "__main__":
+    root = tk.Tk()
+    app = SimVisualizer(root)
+    root.mainloop()

configurations/PID_med+.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.10, 2.0, 0.10]
-integral_multiplier = [0.001, 0.001, 0.001]
-diferential_multiplier = [0.0001, 0.0001, 0.0001]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/PI_med+.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.10, 2.0, 0.10]
-integral_multiplier = [0.001, 0.0001, 0.001]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/proportional_high.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.17, 2.0, 0.17]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/proportional_low.toml

@@ -3,7 +3,7 @@ drone_tick_rate = 600
 
 target_rate = 3.141592
 
-proportional_multiplier = [0.05, 0.5, 0.05]
+proportional_multiplier = [0.005, 0.05, 0.005]
 integral_multiplier = [0.0, 0.0, 0.0]
 diferential_multiplier = [0.0, 0.0, 0.0]
 

configurations/proportional_med+++.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.15, 2.0, 0.15]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/proportional_med++.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.12, 2.0, 0.12]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/proportional_med+.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.11, 2.0, 0.11]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

configurations/proportional_med.toml

@@ -1,13 +0,0 @@
-tickrate = 60000.0
-drone_tick_rate = 600
-
-target_rate = 3.141592
-
-proportional_multiplier = [0.1, 2.0, 0.1]
-integral_multiplier = [0.0, 0.0, 0.0]
-diferential_multiplier = [0.0, 0.0, 0.0]
-
-max_thrust = 2.6
-max_torque = 0.5
-mass = 0.350
-

src/config.rs

@@ -15,6 +15,9 @@ pub struct SimulationConfig {
     pub max_thrust: f32,
     pub max_torque: f32,
 
+    #[serde(default)]
+    pub time_constant: f32,
+
     // Drone
     pub mass: f32,
 }

src/drone.rs

@@ -18,6 +18,7 @@ pub struct MotorCharacteristics {
     pub relative_motor_positions: [na::OPoint<f32, na::Const<3>>; 4],
     pub max_thrust: f32,
     pub max_torque: f32,
+    pub time_constant: f32,
 }
 
 impl Default for MotorCharacteristics {
@@ -41,6 +42,7 @@ impl Default for MotorCharacteristics {
             ],
             max_thrust: 2.6,
             max_torque: 0.5,
+            time_constant: 0.01,
         }
     }
 }
@@ -51,6 +53,8 @@ pub struct Drone {
     pub controller: Box<dyn DroneController>,
     width: f32,
     height: f32,
+    pub current_throttles: [f32; 4],
+    last_time: f32,
 }
 
 fn calculate_drag(velocity: f32, area: f32, constant: f32) -> f32 {
@@ -102,6 +106,8 @@ impl Drone {
             motor_characteristics: motor_characteristics,
             width,
             height,
+            current_throttles: [0.0; 4],
+            last_time: world.get_time(),
         };
     }
 
@@ -116,12 +122,11 @@ impl Drone {
         drag.x = -calculate_drag(velocity.x, side_area, DRAG_CONSTANT);
         drag.y = -calculate_drag(velocity.y, up_area, DRAG_CONSTANT);
         drag.z = -calculate_drag(velocity.z, side_area, DRAG_CONSTANT);
-        // println!("Drag: {:}", drag);
 
         body.add_force(drag, true);
     }
 
-    fn apply_throttles(&mut self, world: &mut World) {
+    fn apply_throttles(&mut self, world: &mut World, dt: f32) {
         let throttles = self.controller.get_motor_throttles();
 
         let drone_rb = world.bodies.get_mut(self.rb_handle).unwrap();
@@ -138,7 +143,16 @@ impl Drone {
             .iter()
             .enumerate()
         {
-            let throttle = throttles[i].clamp(0.0, 1.0);
+            let target_throttle = throttles[i].clamp(0.0, 1.0);
+            // let alpha = (dt / self.motor_characteristics.time_constant).min(1.0); // Linear
+
+            let alpha = 1.0 - (-dt / self.motor_characteristics.time_constant).exp(); // Exp
+                                                                                      // (Accurate)
+
+            self.current_throttles[i] += (target_throttle - self.current_throttles[i]) * alpha;
+
+            let throttle = self.current_throttles[i];
+
             let thrust = self.motor_characteristics.max_thrust * throttle;
             let torque = self.motor_characteristics.max_torque * throttle;
 
@@ -162,8 +176,6 @@ impl Drone {
                 rp::vector![0.0, -torque, 0.0]
             };
             drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true);
-            // println!("Torque: \n{:?}", torque);
-            // println!("Thrust: \n{:?}", thrust);
         }
     }
 
@@ -184,7 +196,9 @@ impl Drone {
 
     pub fn process_tick(&mut self, world: &mut World) {
         self.update_controller(world);
-        self.apply_throttles(world);
+        self.apply_throttles(world, world.get_time() - self.last_time);
         self.apply_drag(world);
+
+        self.last_time = world.get_time();
     }
 }

src/drone/pidcontroller.rs

@@ -6,8 +6,6 @@ use crate::drone::controller::DroneController;
 use crate::drone::JoystickInput;
 use crate::drone::MotorCharacteristics;
 
-const PROPORTIONAL_ONLY: bool = false;
-
 #[derive(Default)]
 pub struct PIDControllerState {
     last_time: f32,
@@ -50,8 +48,6 @@ impl Default for PIDController {
 }
 
 impl PIDController {
-    /// Save log buffers to a CSV file
-
     pub fn set_input(&mut self, input: JoystickInput) {
         self.input = input;
     }
@@ -68,7 +64,7 @@ impl PIDController {
         return coords;
     }
 
-    pub fn get_desired_motor_diffs(&mut self) -> na::Vector3<f32> {
+    fn get_desired_and_error(&mut self) -> [na::Vector3<f32>; 2] {
         let rot = self.state.current_rotation;
         let current = rot.inverse().transform_vector(&self.get_angular_velocity());
         let target = self.get_desired_angular_velocity();
@@ -76,16 +72,7 @@ impl PIDController {
         let error: na::Vector3<f32> = target - current;
         let error_dif: na::Vector3<f32> = error - self.state.last_error;
 
-        let forces_to_apply: na::Vector3<f32> = match PROPORTIONAL_ONLY {
+        let forces_to_apply: na::Vector3<f32> = vector![
-            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,
@@ -95,10 +82,13 @@ impl PIDController {
             error.z * self.proportional_multiplier.z
                 + self.state.error_sum.z * self.integral_multiplier.z
                 + error_dif.z * self.diferential_multiplier.z,
-                ]
+        ];
+
+        return [forces_to_apply, error];
     }
-        };
+
-        return forces_to_apply;
+    pub fn get_desired_motor_diffs(&mut self) -> na::Vector3<f32> {
+        return self.get_desired_and_error()[0];
     }
 }
 
@@ -116,67 +106,22 @@ impl DroneController for PIDController {
     }
     fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {}
     fn get_motor_throttles(&mut self) -> [f32; 4] {
-        let rot = self.state.current_rotation;
+        let [forces_to_apply, error] = self.get_desired_and_error();
-        let current = rot.inverse().transform_vector(&self.get_angular_velocity());
-        let target = self.get_desired_angular_velocity();
-
-        let error: na::Vector3<f32> = target - current;
-        let throttle = self.input.throttle_input;
-
-        let error_dif: na::Vector3<f32> = (error - self.state.last_error) / self.state.dt();
-
-        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.dt();
         self.state.last_error = error;
 
+        let throttle = self.input.throttle_input;
+
         let pitch = forces_to_apply.x;
         let yaw = forces_to_apply.y;
         let roll = forces_to_apply.z;
 
-        let mut motors: [f32; 4] = match self.multiply_mode {
+        let mut motors: [f32; 4] = [
-            true => {
-                let mut t = [
-                    1.0 + yaw - pitch + roll,
-                    1.0 - yaw - pitch - roll,
-                    1.0 + yaw + pitch - roll,
-                    1.0 - yaw + pitch + roll,
-                ];
-                for m in t.iter_mut() {
-                    *m *= self.input.throttle_input;
-                }
-                t
-            }
-            false => {
-                let t = [
             throttle - pitch + yaw + roll,
             throttle - pitch - yaw - roll,
             throttle + pitch + yaw - roll,
             throttle + pitch - yaw + roll,
         ];
-                t
-            }
-        };
 
         let max = motors
             .iter()

src/main_batch.rs

@@ -64,8 +64,8 @@ async fn main() {
 }
 
 async fn run_batch() {
-    fs::remove_dir_all(RESULTS_DIR);
+    let _ = fs::remove_dir_all(RESULTS_DIR);
-    fs::create_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);
@@ -103,10 +103,8 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
         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 {
@@ -119,6 +117,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
         drone::MotorCharacteristics {
             max_thrust: config.max_thrust,
             max_torque: config.max_torque,
+            time_constant: config.time_constant,
             ..Default::default()
         },
         config.mass,
@@ -129,7 +128,6 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
     let mut sim = Simulation::new(
         drone,
         world,
-        false,
         SimMode::Playback(inputs.clone(), 0.0),
         Some(result_file),
         config.drone_tick_rate,
@@ -160,11 +158,10 @@ async fn run_record(output: String) {
     let mut sim = Simulation::new(
         drone,
         world,
-        true,
         SimMode::Record(InputRecording::default(), output),
         None,
         600,
     );
 
-    sim.run().unwrap();
+    sim.run_and_render().await.unwrap();
 }

src/simulation.rs

@@ -34,7 +34,6 @@ pub struct SimulationState {
 pub struct Simulation {
     pub drone: Drone,
     pub world: World,
-    pub renderer: Option<Renderer>,
     pub mode: SimMode,
     results_file: Option<String>,
     drone_tick_rate: u64,
@@ -45,24 +44,13 @@ impl Simulation {
     pub fn new(
         drone: Drone,
         world: World,
-        render: bool,
         mode: SimMode,
         results_file: Option<String>,
         drone_tick_rate: u64,
     ) -> Self {
-        let renderer: Option<Renderer> = match render {
-            true => Some(Renderer::new(Box::new(crate::camera::AttachedCamera::new(
-                drone.rb_handle,
-                na::vector![1.0, 0.0, 0.0],
-                na::vector![0.5, 0.0, 0.0],
-            )))),
-            false => None,
-        };
-
         let mut s = Self {
             drone,
             world,
-            renderer,
             mode,
             results_file,
             drone_tick_rate,
@@ -78,24 +66,26 @@ impl Simulation {
             None,
         );
 
-        if let Some(ren) = &mut s.renderer {
-            ren.light.set_location(
-                na::vector![70.0, 150.0, -90.0].into(),
-                na::vector![-0.4, -0.7, 0.6].into(),
-            );
-            ren.update_light(&s.world);
-        }
-
         return s;
     }
 
-    pub fn run(&mut self) -> Result<(), Box<dyn Error>> {
+    pub async fn run_and_render(&mut self) -> Result<(), Box<dyn Error>> {
+        let mut renderer: Renderer = Renderer::new(Box::new(crate::camera::AttachedCamera::new(
+            self.drone.rb_handle,
+            na::vector![1.0, 0.0, 0.0],
+            na::vector![0.5, 0.0, 0.0],
+        )));
+
+        renderer.light.set_location(
+            na::vector![70.0, 150.0, -90.0].into(),
+            na::vector![-0.4, -0.7, 0.6].into(),
+        );
+        renderer.update_light(&self.world);
+
         let mut current_input: JoystickInput;
 
         loop {
-            if let Some(renderer) = &mut self.renderer {
             renderer.update_camera(&self.world);
-            }
 
             // --- Input handling ---
             let current_time = self.world.get_time() as f32;
@@ -182,19 +172,108 @@ impl Simulation {
             });
 
             // --- Rendering ---
-            if let Some(renderer) = &mut self.renderer {
+            if self.world.tick % ((self.world.integration_parameters.inv_dt() / 60.0) as u64) == 0 {
-                if self.world.tick % ((self.world.integration_parameters.inv_dt() / 60.0) as u64)
-                    == 0
-                {
                 renderer.draw(&mut self.world);
                 mq::next_frame();
             }
         }
-        }
 
         self.shutdown()?;
         Ok(())
     }
+
+    pub fn run(&mut self) -> Result<(), Box<dyn Error>> {
+        let mut current_input: JoystickInput;
+
+        loop {
+            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;
+                    current_input = recording.get_input(playback_time);
+
+                    if recording.ended(playback_time) {
+                        println!("Playback ended.");
+                        break;
+                    }
+                }
+            }
+
+            // --- Physics ---
+            self.world.step();
+            if self.world.tick
+                % ((self.world.integration_parameters.inv_dt() / self.drone_tick_rate as f32)
+                    as u64)
+                == 0
+            {
+                if let Some(cont) = self
+                    .drone
+                    .controller
+                    .as_mut_any()
+                    .downcast_mut::<crate::drone::pidcontroller::PIDController>()
+                {
+                    cont.set_input(current_input);
+                }
+                self.drone.process_tick(&mut self.world);
+            }
+
+            let target_angular_vel: na::Vector3<f32>;
+            let desired_motor_diff: na::Vector3<f32>;
+            let applied_motor_diff: na::Vector3<f32>;
+            if let Some(cont) = self
+                .drone
+                .controller
+                .as_mut_any()
+                .downcast_mut::<crate::drone::pidcontroller::PIDController>()
+            {
+                desired_motor_diff = cont.get_desired_motor_diffs();
+                target_angular_vel = cont.get_desired_angular_velocity();
+
+                /*
+                let t = [
+                    throttle - pitch + yaw + roll,
+                    throttle - pitch - yaw - roll,
+                    throttle + pitch + yaw - roll,
+                    throttle + pitch - yaw + roll,
+                ];
+                */
+                let m = self.drone.current_throttles;
+                applied_motor_diff = na::vector![
+                    (m[2] + m[3] - m[0] - m[1]),
+                    (m[0] + m[2] - m[1] - m[3]),
+                    (m[0] + m[3] - m[1] - m[2])
+                ] / 2.0;
+            } else {
+                target_angular_vel = na::vector![0.0, 0.0, 0.0];
+                desired_motor_diff = na::vector![0.0, 0.0, 0.0];
+                applied_motor_diff = na::vector![0.0, 0.0, 0.0];
+            }
+
+            self.state.data_results.push(DataResultRecord {
+                time: self.world.get_time(),
+                current_angular_velocity: self
+                    .drone
+                    .get_rot(&self.world)
+                    .inverse()
+                    .transform_vector(&self.drone.get_angvel(&self.world)),
+                target_angular_velocity: target_angular_vel,
+                applied_motor_offset: applied_motor_diff,
+                desired_motor_offset: desired_motor_diff,
+            });
+        }
+
+        self.shutdown()?;
+        Ok(())
+    }
+
     pub fn save_logs_to_csv(&self, path: &str) -> std::io::Result<()> {
         use std::fs::File;
         use std::io::{BufWriter, Write};