From 79734a5394c2f8fb707098502817c0cd93913e90 Mon Sep 17 00:00:00 2001 From: franchioping Date: Wed, 4 Feb 2026 23:01:09 +0000 Subject: [PATCH] modules almosttt working --- configurations/all.toml | 47 +++++ configurations/pitch.toml | 47 +++++ configurations/roll.toml | 47 +++++ configurations/sim_std_mot_std_low_01.toml | 14 -- configurations/sim_std_mot_std_low_02.toml | 14 -- configurations/sim_std_mot_std_low_03.toml | 14 -- configurations/sim_std_mot_std_low_04.toml | 14 -- configurations/sim_std_mot_std_low_05.toml | 14 -- configurations/sim_std_mot_std_low_06.toml | 14 -- configurations/sim_std_mot_std_low_07.toml | 14 -- configurations/sim_std_mot_std_low_08.toml | 14 -- configurations/sim_std_mot_std_low_09.toml | 14 -- configurations/sim_std_mot_std_low_10.toml | 14 -- configurations/sim_std_mot_tc_low_01.toml | 14 -- configurations/sim_std_mot_tc_low_02.toml | 14 -- configurations/sim_std_mot_tc_low_03.toml | 14 -- configurations/sim_std_mot_tc_low_04.toml | 14 -- configurations/sim_std_mot_tc_low_05.toml | 14 -- configurations/sim_std_mot_tc_low_06.toml | 14 -- configurations/sim_std_mot_tc_low_07.toml | 14 -- configurations/sim_std_mot_tc_low_08.toml | 14 -- configurations/sim_std_mot_tc_low_09.toml | 14 -- configurations/sim_std_mot_tc_low_10.toml | 14 -- configurations/yaw.toml | 47 +++++ src/config.rs | 70 ++++--- src/drone.rs | 8 +- src/drone/pidcontroller.rs | 4 +- src/drone/stacked.rs | 221 +++++++++++++++++++++ src/main.rs | 1 + src/main_batch.rs | 49 +---- src/rendering.rs | 2 +- src/rendering/ui.rs | 2 +- src/simulation.rs | 28 +-- 33 files changed, 479 insertions(+), 374 deletions(-) create mode 100644 configurations/all.toml create mode 100644 configurations/pitch.toml create mode 100644 configurations/roll.toml delete mode 100644 configurations/sim_std_mot_std_low_01.toml delete mode 100644 configurations/sim_std_mot_std_low_02.toml delete mode 100644 configurations/sim_std_mot_std_low_03.toml delete mode 100644 configurations/sim_std_mot_std_low_04.toml delete mode 100644 configurations/sim_std_mot_std_low_05.toml delete mode 100644 configurations/sim_std_mot_std_low_06.toml delete mode 100644 configurations/sim_std_mot_std_low_07.toml delete mode 100644 configurations/sim_std_mot_std_low_08.toml delete mode 100644 configurations/sim_std_mot_std_low_09.toml delete mode 100644 configurations/sim_std_mot_std_low_10.toml delete mode 100644 configurations/sim_std_mot_tc_low_01.toml delete mode 100644 configurations/sim_std_mot_tc_low_02.toml delete mode 100644 configurations/sim_std_mot_tc_low_03.toml delete mode 100644 configurations/sim_std_mot_tc_low_04.toml delete mode 100644 configurations/sim_std_mot_tc_low_05.toml delete mode 100644 configurations/sim_std_mot_tc_low_06.toml delete mode 100644 configurations/sim_std_mot_tc_low_07.toml delete mode 100644 configurations/sim_std_mot_tc_low_08.toml delete mode 100644 configurations/sim_std_mot_tc_low_09.toml delete mode 100644 configurations/sim_std_mot_tc_low_10.toml create mode 100644 configurations/yaw.toml create mode 100644 src/drone/stacked.rs diff --git a/configurations/all.toml b/configurations/all.toml new file mode 100644 index 0000000..d82ffc3 --- /dev/null +++ b/configurations/all.toml @@ -0,0 +1,47 @@ + + + +tickrate = 6000.0 +drone_tick_rate = 600 +max_thrust = 2.6 +max_torque = 0.5 +mass = 0.350 + +# The Stack: First layer computes target speed, second layer computes motor torque +layers = [ + # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] }, + { type = "Rate", max_rate = 3.14, kp = [ + 0.01, + 0.1, + 0.01, + ], ki = [ + 0.0, + 0.0, + 0.0, + ], kd = [ + 0.0, + 0.0, + 0.0, + ] }, +] + +# + (map[0] * setpoint.x) // Roll +# + (map[1] * setpoint.y) // Yaw +# + (map[2] * setpoint.z); // Pitch + +# /* +# * Motor position indices +# * ^ - Front +# * | +# * | +# * 1 --- 0 +# * | | +# * | | +# * 2 --- 3 +# */ +motor_map = [ + [-1.0, 1.0, 1.0], # Front Right + [1.0, -1.0, 1.0], # Front Left + [1.0, 1.0, -1.0], # Rear Left + [-1.0, -1.0, -1.0], # Rear Right +] diff --git a/configurations/pitch.toml b/configurations/pitch.toml new file mode 100644 index 0000000..613c5cc --- /dev/null +++ b/configurations/pitch.toml @@ -0,0 +1,47 @@ + + + +tickrate = 6000.0 +drone_tick_rate = 600 +max_thrust = 2.6 +max_torque = 0.5 +mass = 0.350 + +# The Stack: First layer computes target speed, second layer computes motor torque +layers = [ + # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] }, + { type = "Rate", max_rate = 3.14, kp = [ + 0.0, + 0.0, + 0.01, + ], ki = [ + 0.0, + 0.0, + 0.0, + ], kd = [ + 0.0, + 0.0, + 0.0, + ] }, +] + +# + (map[0] * setpoint.x) // Roll +# + (map[1] * setpoint.y) // Yaw +# + (map[2] * setpoint.z); // Pitch + +# /* +# * Motor position indices +# * ^ - Front +# * | +# * | +# * 1 --- 0 +# * | | +# * | | +# * 2 --- 3 +# */ +motor_map = [ + [-1.0, 1.0, 1.0], # Front Right + [1.0, -1.0, 1.0], # Front Left + [1.0, 1.0, -1.0], # Rear Left + [-1.0, -1.0, -1.0], # Rear Right +] diff --git a/configurations/roll.toml b/configurations/roll.toml new file mode 100644 index 0000000..b2930bb --- /dev/null +++ b/configurations/roll.toml @@ -0,0 +1,47 @@ + + + +tickrate = 6000.0 +drone_tick_rate = 600 +max_thrust = 2.6 +max_torque = 0.5 +mass = 0.350 + +# The Stack: First layer computes target speed, second layer computes motor torque +layers = [ + # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] }, + { type = "Rate", max_rate = 3.14, kp = [ + 0.01, + 0.0, + 0.0, + ], ki = [ + 0.0, + 0.0, + 0.0, + ], kd = [ + 0.0, + 0.0, + 0.0, + ] }, +] + +# + (map[0] * setpoint.x) // Roll +# + (map[1] * setpoint.y) // Yaw +# + (map[2] * setpoint.z); // Pitch + +# /* +# * Motor position indices +# * ^ - Front +# * | +# * | +# * 1 --- 0 +# * | | +# * | | +# * 2 --- 3 +# */ +motor_map = [ + [-1.0, 1.0, 1.0], # Front Right + [1.0, -1.0, 1.0], # Front Left + [1.0, 1.0, -1.0], # Rear Left + [-1.0, -1.0, -1.0], # Rear Right +] diff --git a/configurations/sim_std_mot_std_low_01.toml b/configurations/sim_std_mot_std_low_01.toml deleted file mode 100644 index cd74913..0000000 --- a/configurations/sim_std_mot_std_low_01.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.005, 0.005, 0.005] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_02.toml b/configurations/sim_std_mot_std_low_02.toml deleted file mode 100644 index 393d6ad..0000000 --- a/configurations/sim_std_mot_std_low_02.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.01, 0.01, 0.01] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_03.toml b/configurations/sim_std_mot_std_low_03.toml deleted file mode 100644 index 05ec016..0000000 --- a/configurations/sim_std_mot_std_low_03.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.015, 0.015, 0.015] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_04.toml b/configurations/sim_std_mot_std_low_04.toml deleted file mode 100644 index 9c51409..0000000 --- a/configurations/sim_std_mot_std_low_04.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.02, 0.02, 0.02] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_05.toml b/configurations/sim_std_mot_std_low_05.toml deleted file mode 100644 index 6e4ec30..0000000 --- a/configurations/sim_std_mot_std_low_05.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.025, 0.025, 0.025] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_06.toml b/configurations/sim_std_mot_std_low_06.toml deleted file mode 100644 index a0b8247..0000000 --- a/configurations/sim_std_mot_std_low_06.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.03, 0.03, 0.03] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_07.toml b/configurations/sim_std_mot_std_low_07.toml deleted file mode 100644 index 9c8b285..0000000 --- a/configurations/sim_std_mot_std_low_07.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.035, 0.035, 0.035] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_08.toml b/configurations/sim_std_mot_std_low_08.toml deleted file mode 100644 index 43f8f99..0000000 --- a/configurations/sim_std_mot_std_low_08.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.04, 0.04, 0.04] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_09.toml b/configurations/sim_std_mot_std_low_09.toml deleted file mode 100644 index 3151931..0000000 --- a/configurations/sim_std_mot_std_low_09.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.045, 0.045, 0.045] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_std_low_10.toml b/configurations/sim_std_mot_std_low_10.toml deleted file mode 100644 index 0a96a4a..0000000 --- a/configurations/sim_std_mot_std_low_10.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - - - -target_rate = 3.141592 -proportional_multiplier = [0.05, 0.05, 0.05] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_01.toml b/configurations/sim_std_mot_tc_low_01.toml deleted file mode 100644 index d75a43e..0000000 --- a/configurations/sim_std_mot_tc_low_01.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.005, 0.005, 0.005] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_02.toml b/configurations/sim_std_mot_tc_low_02.toml deleted file mode 100644 index 1059a49..0000000 --- a/configurations/sim_std_mot_tc_low_02.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.01, 0.01, 0.01] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_03.toml b/configurations/sim_std_mot_tc_low_03.toml deleted file mode 100644 index 2614f40..0000000 --- a/configurations/sim_std_mot_tc_low_03.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.015, 0.015, 0.015] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_04.toml b/configurations/sim_std_mot_tc_low_04.toml deleted file mode 100644 index d07903c..0000000 --- a/configurations/sim_std_mot_tc_low_04.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.02, 0.02, 0.02] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_05.toml b/configurations/sim_std_mot_tc_low_05.toml deleted file mode 100644 index 2e34077..0000000 --- a/configurations/sim_std_mot_tc_low_05.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.025, 0.025, 0.025] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_06.toml b/configurations/sim_std_mot_tc_low_06.toml deleted file mode 100644 index 0fa2154..0000000 --- a/configurations/sim_std_mot_tc_low_06.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.03, 0.03, 0.03] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_07.toml b/configurations/sim_std_mot_tc_low_07.toml deleted file mode 100644 index 4b5fc4b..0000000 --- a/configurations/sim_std_mot_tc_low_07.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.035, 0.035, 0.035] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_08.toml b/configurations/sim_std_mot_tc_low_08.toml deleted file mode 100644 index 81d2c33..0000000 --- a/configurations/sim_std_mot_tc_low_08.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.04, 0.04, 0.04] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_09.toml b/configurations/sim_std_mot_tc_low_09.toml deleted file mode 100644 index cb41d5d..0000000 --- a/configurations/sim_std_mot_tc_low_09.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.045, 0.045, 0.045] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/sim_std_mot_tc_low_10.toml b/configurations/sim_std_mot_tc_low_10.toml deleted file mode 100644 index 1458ca0..0000000 --- a/configurations/sim_std_mot_tc_low_10.toml +++ /dev/null @@ -1,14 +0,0 @@ -tickrate = 6000.0 -drone_tick_rate = 600 - -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant=0.01 - - -target_rate = 3.141592 -proportional_multiplier = [0.05, 0.05, 0.05] -integral_multiplier = [0.0, 0.0, 0.0] -diferential_multiplier = [0.0, 0.0, 0.0] diff --git a/configurations/yaw.toml b/configurations/yaw.toml new file mode 100644 index 0000000..e65454a --- /dev/null +++ b/configurations/yaw.toml @@ -0,0 +1,47 @@ + + + +tickrate = 6000.0 +drone_tick_rate = 600 +max_thrust = 2.6 +max_torque = 2.0 +mass = 0.350 + +# The Stack: First layer computes target speed, second layer computes motor torque +layers = [ + # { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] }, + { type = "Rate", max_rate = 3.14, kp = [ + 0.0, + 0.01, + 0.0, + ], ki = [ + 0.0, + 0.0, + 0.0, + ], kd = [ + 0.0, + 0.0, + 0.0, + ] }, +] + +# + (map[0] * setpoint.x) // Roll +# + (map[1] * setpoint.y) // Yaw +# + (map[2] * setpoint.z); // Pitch + +# /* +# * Motor position indices +# * ^ - Front +# * | +# * | +# * 1 --- 0 +# * | | +# * | | +# * 2 --- 3 +# */ +motor_map = [ + [-1.0, 1.0, 1.0], # Front Right + [1.0, -1.0, 1.0], # Front Left + [1.0, 1.0, -1.0], # Rear Left + [-1.0, -1.0, -1.0], # Rear Right +] diff --git a/src/config.rs b/src/config.rs index a4cd32d..868568d 100644 --- a/src/config.rs +++ b/src/config.rs @@ -1,49 +1,59 @@ use rapier3d::prelude::*; +use serde::Deserialize; -#[derive(Debug, serde::Deserialize)] +#[derive(Debug, Deserialize, Clone)] +pub struct PidConfig { + pub kp: [f32; 3], + pub ki: [f32; 3], + pub kd: [f32; 3], +} + +#[derive(Debug, Deserialize, Clone)] +#[serde(tag = "type")] +pub enum LayerConfig { + /// Controls angular velocity (Input: joystick/previous layer -> Output: torque) + Rate { + #[serde(flatten)] + pid: PidConfig, + max_rate: f32, + }, + /// Controls orientation (Input: joystick -> Output: desired angular velocity) + Angle { + #[serde(flatten)] + pid: PidConfig, + max_angle: f32, + }, +} + +#[derive(Debug, Deserialize, Clone)] pub struct SimulationConfig { pub tickrate: f32, pub drone_tick_rate: u64, - // PID - pub target_rate: f32, - pub proportional_multiplier: [f32; 3], - pub integral_multiplier: [f32; 3], - pub diferential_multiplier: [f32; 3], + // --- Modular Controller Stack --- + // The order of this Vec defines the stack (e.g., [Angle, Rate]) + pub layers: Vec, - // Motors + /// Maps [Pitch, Yaw, Roll] to each of the 4 motors + pub motor_map: [[f32; 3]; 4], + + // Motors & Physics pub max_thrust: f32, pub max_torque: f32, #[serde(default)] pub time_constant: f32, - - // Drone pub mass: f32, } -impl SimulationConfig { - pub fn proportional(&self) -> Vector { - vector![ - self.proportional_multiplier[0], - self.proportional_multiplier[1], - self.proportional_multiplier[2] - ] +impl PidConfig { + pub fn p_vec(&self) -> Vector { + vector![self.kp[0], self.kp[1], self.kp[2]] } - - pub fn integral(&self) -> Vector { - vector![ - self.integral_multiplier[0], - self.integral_multiplier[1], - self.integral_multiplier[2] - ] + pub fn i_vec(&self) -> Vector { + vector![self.ki[0], self.ki[1], self.ki[2]] } - - pub fn diferential(&self) -> Vector { - vector![ - self.diferential_multiplier[0], - self.diferential_multiplier[1], - self.diferential_multiplier[2] - ] + pub fn d_vec(&self) -> Vector { + vector![self.kd[0], self.kd[1], self.kd[2]] } } diff --git a/src/drone.rs b/src/drone.rs index 5be867d..8c7a645 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -7,6 +7,7 @@ pub mod controller; pub mod fpvcontroller; pub mod input; pub mod pidcontroller; +pub mod stacked; use controller::*; pub use input::JoystickInput; @@ -36,9 +37,9 @@ impl Default for MotorCharacteristics { */ relative_motor_positions: [ rp::point![5.0, 0.0, 5.0], - rp::point![-5.0, 0.0, 5.0], - rp::point![-5.0, 0.0, -5.0], rp::point![5.0, 0.0, -5.0], + rp::point![-5.0, 0.0, -5.0], + rp::point![-5.0, 0.0, 5.0], ], max_thrust: 2.6, max_torque: 0.5, @@ -80,7 +81,8 @@ impl Drone { * A Poor Man's fluid simulation :D */ // .linear_damping(0.2) // Damps velocity slowly - .angular_damping(0.2) // Damps angular velocity slowly + .angular_damping(0.1) // Damps angular velocity slowly + .gyroscopic_forces_enabled(true) .build(), ); let width = 0.40; diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs index 19ec0c0..cb62571 100644 --- a/src/drone/pidcontroller.rs +++ b/src/drone/pidcontroller.rs @@ -2,9 +2,9 @@ use nalgebra::{self as na, vector}; use std::{any::Any, f32}; -use crate::drone::controller::DroneController; use crate::drone::JoystickInput; use crate::drone::MotorCharacteristics; +use crate::drone::controller::DroneController; #[derive(Default)] pub struct PIDControllerState { @@ -64,7 +64,7 @@ impl PIDController { return coords; } - fn get_desired_and_error(&mut self) -> [na::Vector3; 2] { + pub 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(); diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs new file mode 100644 index 0000000..9d4949e --- /dev/null +++ b/src/drone/stacked.rs @@ -0,0 +1,221 @@ +#![allow(dead_code)] + +use nalgebra::{self as na, vector}; +use std::{any::Any, f32}; + +use crate::drone::controller::DroneController; +use crate::drone::JoystickInput; + +use crate::config::*; + +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, + + // State + drone_state: DroneState, + input: JoystickInput, + last_time: f32, + current_time: f32, +} + +impl StackedController { + pub fn set_input(&mut self, inp: JoystickInput) { + self.input = inp; + } + pub fn new(config: SimulationConfig) -> Self { + let mut modules = Vec::new(); + + for layer in &config.layers { + match layer { + LayerConfig::Angle { pid, max_angle } => { + modules.push(ControllerModule::Angle { + processor: PidProcessor::new(pid), + max_angle: *max_angle, + }); + } + LayerConfig::Rate { pid, max_rate } => { + modules.push(ControllerModule::Rate { + processor: PidProcessor::new(pid), + max_rate: *max_rate, + }); + } + } + } + + Self { + modules, + config, + input: JoystickInput::default(), + drone_state: DroneState { + rotation: na::UnitQuaternion::identity(), + angular_velocity: na::Vector3::zeros(), + }, + last_time: 0.0, + current_time: 0.0, + } + } +} + +impl DroneController for StackedController { + fn set_rotation(&mut self, rot: na::UnitQuaternion) { + self.drone_state.rotation = rot; + } + fn set_angular_velocity(&mut self, vel: na::Vector3) { + self.drone_state.angular_velocity = vel; + } + fn set_time(&mut self, t: f32) { + self.last_time = self.current_time; + self.current_time = t; + } + + 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 + } + + fn as_any(&self) -> &dyn Any { + self + } + fn as_mut_any(&mut self) -> &mut dyn Any { + 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 9b96b8c..06bb4ec 100644 --- a/src/main.rs +++ b/src/main.rs @@ -8,6 +8,7 @@ mod camera; mod drone; mod rendering; +mod config; mod graphics_util; use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer}; diff --git a/src/main_batch.rs b/src/main_batch.rs index 51acfdc..6e3352b 100644 --- a/src/main_batch.rs +++ b/src/main_batch.rs @@ -1,5 +1,3 @@ -use macroquad::prelude as mq; - mod engine; use engine::*; mod camera; @@ -22,48 +20,15 @@ const INPUTS_DIR: &str = "inputs/"; const CONFIGS_DIR: &str = "configurations/"; const RESULTS_DIR: &str = "results/"; -fn window_conf() -> mq::Conf { - mq::Conf { - window_title: "RustDroneSim".to_owned(), - window_resizable: true, - platform: mq::miniquad::conf::Platform { - ..Default::default() - }, - ..Default::default() - } -} - use std::env; use std::path::PathBuf; use std::thread::JoinHandle; -enum RunMode { - Batch, - Record { output: String }, +fn main() { + run_batch(); } -fn parse_cli() -> RunMode { - let mut args = env::args().skip(1); - - match args.next().as_deref() { - Some("record") => { - let output = args.next().expect("record mode requires output file path"); - RunMode::Record { output } - } - Some("batch") | None => RunMode::Batch, - Some(other) => panic!("Unknown mode: {}", other), - } -} - -#[macroquad::main(window_conf)] -async fn main() { - match parse_cli() { - RunMode::Batch => run_batch().await, - RunMode::Record { output } => run_record(output).await, - } -} - -async fn run_batch() { +fn run_batch() { let _ = fs::remove_dir_all(RESULTS_DIR); let _ = fs::create_dir_all(RESULTS_DIR); @@ -107,13 +72,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) { let drone = drone::Drone::new( &mut world, - Box::new(drone::pidcontroller::PIDController { - target_rate: config.target_rate, - proportional_multiplier: config.proportional(), - integral_multiplier: config.integral(), - diferential_multiplier: config.diferential(), - ..Default::default() - }), + Box::new(drone::stacked::StackedController::new(config.clone())), drone::MotorCharacteristics { max_thrust: config.max_thrust, max_torque: config.max_torque, diff --git a/src/rendering.rs b/src/rendering.rs index 073b8fb..6af73f1 100644 --- a/src/rendering.rs +++ b/src/rendering.rs @@ -91,8 +91,8 @@ impl MaterialUni { } } pub fn as_type(&self) -> mq::UniformType { - use mq::UniformType::*; use MaterialUni::*; + use mq::UniformType::*; match self { RenderNormalsBool => Int1, RenderShadowsBool => Int1, diff --git a/src/rendering/ui.rs b/src/rendering/ui.rs index c080dca..53fa747 100644 --- a/src/rendering/ui.rs +++ b/src/rendering/ui.rs @@ -1,6 +1,6 @@ use crate::rendering::{self, MaterialUni}; use macroquad::prelude::*; -use macroquad::ui::{hash, Ui}; +use macroquad::ui::{Ui, hash}; use strum::IntoEnumIterator; diff --git a/src/simulation.rs b/src/simulation.rs index 64986fa..d44a0e3 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -218,7 +218,7 @@ impl Simulation { .drone .controller .as_mut_any() - .downcast_mut::() + .downcast_mut::() { cont.set_input(current_input); } @@ -246,18 +246,22 @@ impl Simulation { 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]; + target_angular_vel = na::vector![ + current_input.roll_input, + current_input.yaw_input, + current_input.pitch_input, + ] * 3.14; desired_motor_diff = na::vector![0.0, 0.0, 0.0]; - applied_motor_diff = na::vector![0.0, 0.0, 0.0]; } + let m = self.drone.current_throttles; + applied_motor_diff = na::vector![ + (m[1] + m[2] - m[0] - m[3]), + (m[0] + m[2] - m[1] - m[3]), + (m[2] + m[3] - m[0] - m[1]) + ] / 2.0; + self.state.data_results.push(DataResultRecord { time: self.world.get_time(), current_angular_velocity: self @@ -282,9 +286,9 @@ impl Simulation { let mut writer = BufWriter::with_capacity(1 << 20, file); // 1 MB buffer writeln!( - writer, - "time,target_x,target_y,target_z,current_x,current_y,current_z,error_x,error_y,error_z,mot_x,mot_y,mot_z,dmot_x,dmot_y,dmot_z" - )?; + writer, + "time,target_x,target_y,target_z,current_x,current_y,current_z,error_x,error_y,error_z,mot_x,mot_y,mot_z,dmot_x,dmot_y,dmot_z" + )?; for entry in &self.state.data_results { let tg = entry.target_angular_velocity;