From 2c5955b8b596f283ecd76e939259c7450a4b04fa Mon Sep 17 00:00:00 2001 From: franchioping Date: Wed, 4 Feb 2026 17:54:41 +0000 Subject: [PATCH] WIP --- analyze.py | 176 ++++++++++++++++++------ configurations/PID_med+.toml | 13 -- configurations/PI_med+.toml | 13 -- configurations/proportional_high.toml | 13 -- configurations/proportional_low.toml | 2 +- configurations/proportional_med+++.toml | 13 -- configurations/proportional_med++.toml | 13 -- configurations/proportional_med+.toml | 13 -- configurations/proportional_med.toml | 13 -- src/config.rs | 3 + src/drone.rs | 26 +++- src/drone/pidcontroller.rs | 109 ++++----------- src/main_batch.rs | 11 +- src/simulation.rs | 141 ++++++++++++++----- 14 files changed, 299 insertions(+), 260 deletions(-) delete mode 100644 configurations/PID_med+.toml delete mode 100644 configurations/PI_med+.toml delete mode 100644 configurations/proportional_high.toml delete mode 100644 configurations/proportional_med+++.toml delete mode 100644 configurations/proportional_med++.toml delete mode 100644 configurations/proportional_med+.toml delete mode 100644 configurations/proportional_med.toml diff --git a/analyze.py b/analyze.py index 6064b66..537cfcf 100644 --- a/analyze.py +++ b/analyze.py @@ -1,56 +1,148 @@ -import pandas as pd -import matplotlib.pyplot as plt +import tkinter as tk from pathlib import Path +from tkinter import messagebox, ttk -# -------- File picker -------- -RESULTS_DIR = Path("results") +import matplotlib.pyplot as plt +import pandas as pd +from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg, NavigationToolbar2Tk -csv_files = sorted(RESULTS_DIR.glob("*.csv")) -if not csv_files: - raise FileNotFoundError("No CSV files found in results/") +class SimVisualizer: + def __init__(self, root: tk.Tk): + self.root = root + self.root.title("Simulation Data Viewer") -print("Available result files:") -for i, f in enumerate(csv_files): - print(f"[{i}] {f.name}") + # --- Protocol Handler --- + # This handles the "X" button on the window frame + self.root.protocol("WM_DELETE_WINDOW", self.quit_app) -choice = int(input("Select file number: ")) -csv_path = csv_files[choice] + self.results_dir = Path("results") + self.df = None + self.current_file = None -print(f"\nLoading: {csv_path}\n") + self.setup_ui() + self.refresh_file_list() -# -------- Load CSV -------- -df = pd.read_csv(csv_path) + def setup_ui(self): + # 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 -------- -for coord in ["x"]: - plt.figure() - 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}") + # --- Sidebar Frame --- + # Note: We still define a width, but it's now the *initial* width + sidebar = ttk.Frame(self.paned_window, width=300, padding="10") - plt.xlabel("Time (s)") - plt.ylabel("Angular velocity & Motor Offset") - plt.title(f"{csv_path.name} — {coord.upper()} axis") - plt.legend(loc="upper right") - plt.grid(True) + # 2. Add the sidebar to the PanedWindow + # 'weight=0' keeps it from growing automatically when the window is resized + self.paned_window.add(sidebar, weight=0) - ymin, ymax = plt.ylim() - plt.ylim(min(ymin, -1), max(ymax, 1)) + ttk.Label(sidebar, text="Result Files", font=("Helvetica", 12, "bold")).pack( + 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("<>", self.on_file_select) -# -------- 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(f"PID Error — {csv_path.name}") -# plt.legend(loc="upper right") -# plt.grid(True) -# plt.show() + ttk.Separator(sidebar, orient="horizontal").pack(fill="x", pady=10) + 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() diff --git a/configurations/PID_med+.toml b/configurations/PID_med+.toml deleted file mode 100644 index 6409139..0000000 --- a/configurations/PID_med+.toml +++ /dev/null @@ -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 - diff --git a/configurations/PI_med+.toml b/configurations/PI_med+.toml deleted file mode 100644 index 6f14766..0000000 --- a/configurations/PI_med+.toml +++ /dev/null @@ -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 - diff --git a/configurations/proportional_high.toml b/configurations/proportional_high.toml deleted file mode 100644 index f856e5f..0000000 --- a/configurations/proportional_high.toml +++ /dev/null @@ -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 - diff --git a/configurations/proportional_low.toml b/configurations/proportional_low.toml index 6840723..8998145 100644 --- a/configurations/proportional_low.toml +++ b/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] diff --git a/configurations/proportional_med+++.toml b/configurations/proportional_med+++.toml deleted file mode 100644 index e127c7d..0000000 --- a/configurations/proportional_med+++.toml +++ /dev/null @@ -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 - diff --git a/configurations/proportional_med++.toml b/configurations/proportional_med++.toml deleted file mode 100644 index bab915c..0000000 --- a/configurations/proportional_med++.toml +++ /dev/null @@ -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 - diff --git a/configurations/proportional_med+.toml b/configurations/proportional_med+.toml deleted file mode 100644 index a16f05a..0000000 --- a/configurations/proportional_med+.toml +++ /dev/null @@ -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 - diff --git a/configurations/proportional_med.toml b/configurations/proportional_med.toml deleted file mode 100644 index 221f24d..0000000 --- a/configurations/proportional_med.toml +++ /dev/null @@ -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 - diff --git a/src/config.rs b/src/config.rs index 79e1286..a4cd32d 100644 --- a/src/config.rs +++ b/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, } diff --git a/src/drone.rs b/src/drone.rs index 66ff3dc..27a243e 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -18,6 +18,7 @@ pub struct MotorCharacteristics { pub relative_motor_positions: [na::OPoint>; 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, 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(); } } diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs index 14e0e60..19ec0c0 100644 --- a/src/drone/pidcontroller.rs +++ b/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 { + fn get_desired_and_error(&mut self) -> [na::Vector3; 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,29 +72,23 @@ impl PIDController { let error: na::Vector3 = target - current; let error_dif: na::Vector3 = error - self.state.last_error; - let forces_to_apply: na::Vector3 = 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, - ] - } - }; - return forces_to_apply; + let forces_to_apply: na::Vector3 = 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, + ]; + + return [forces_to_apply, error]; + } + + pub fn get_desired_motor_diffs(&mut self) -> na::Vector3 { + 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 current = rot.inverse().transform_vector(&self.get_angular_velocity()); - let target = self.get_desired_angular_velocity(); - - let error: na::Vector3 = target - current; - let throttle = self.input.throttle_input; - - let error_dif: na::Vector3 = (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, - ] - } - }; + let [forces_to_apply, error] = self.get_desired_and_error(); 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 { - 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 mut motors: [f32; 4] = [ + throttle - pitch + yaw + roll, + throttle - pitch - yaw - roll, + throttle + pitch + yaw - roll, + throttle + pitch - yaw + roll, + ]; let max = motors .iter() diff --git a/src/main_batch.rs b/src/main_batch.rs index 5529208..51acfdc 100644 --- a/src/main_batch.rs +++ b/src/main_batch.rs @@ -64,8 +64,8 @@ async fn main() { } async fn run_batch() { - fs::remove_dir_all(RESULTS_DIR); - fs::create_dir_all(RESULTS_DIR); + 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); @@ -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(); } diff --git a/src/simulation.rs b/src/simulation.rs index a25f8d7..9e3cf73 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -34,7 +34,6 @@ pub struct SimulationState { pub struct Simulation { pub drone: Drone, pub world: World, - pub renderer: Option, pub mode: SimMode, results_file: Option, drone_tick_rate: u64, @@ -45,24 +44,13 @@ impl Simulation { pub fn new( drone: Drone, world: World, - render: bool, mode: SimMode, results_file: Option, drone_tick_rate: u64, ) -> Self { - let renderer: Option = 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> { + pub async fn run_and_render(&mut self) -> Result<(), Box> { + 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); - } + 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 - { - renderer.draw(&mut self.world); - mq::next_frame(); - } + 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> { + 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::() + { + cont.set_input(current_input); + } + self.drone.process_tick(&mut self.world); + } + + let target_angular_vel: na::Vector3; + let desired_motor_diff: na::Vector3; + let applied_motor_diff: na::Vector3; + if let Some(cont) = self + .drone + .controller + .as_mut_any() + .downcast_mut::() + { + 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};