From 6f61d4ab432115edf5bd90e2c697b6ac21bd5b34 Mon Sep 17 00:00:00 2001 From: franchioping Date: Mon, 23 Feb 2026 08:23:52 +0000 Subject: [PATCH] idk this is math class --- Cargo.lock | 116 +++++++++++ Cargo.toml | 8 + configurations/pid_cont/rate_01.toml | 2 +- configurations/pid_cont/rate_02.toml | 2 +- configurations/pid_cont/rate_03.toml | 2 +- inputs/ad.csv | 50 ----- inputs/leftright.csv | 34 ---- inputs/test.csv | 144 ------------- inputs/updown.csv | 37 ---- src/config.rs | 2 + src/drone/input.rs | 292 +++++++++++---------------- src/drone/stacked.rs | 87 +++++--- src/drone/stacked/modules.rs | 13 +- src/main.rs | 2 +- src/main_record.rs | 95 +++++++++ src/simulation.rs | 14 +- 16 files changed, 425 insertions(+), 475 deletions(-) delete mode 100644 inputs/ad.csv delete mode 100644 inputs/leftright.csv delete mode 100644 inputs/test.csv delete mode 100644 inputs/updown.csv create mode 100644 src/main_record.rs diff --git a/Cargo.lock b/Cargo.lock index f1bebea..bd49e46 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -6,6 +6,7 @@ version = 4 name = "RustPhysicsMQ" version = "0.1.0" dependencies = [ + "clap", "clearscreen", "csv", "gilrs", @@ -15,6 +16,7 @@ dependencies = [ "rand 0.9.2", "rapier3d", "serde", + "serde_json", "serde_with", "strum", "toml", @@ -41,6 +43,56 @@ dependencies = [ "libc", ] +[[package]] +name = "anstream" +version = "0.6.21" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "43d5b281e737544384e969a5ccad3f1cdd24b48086a0fc1b2a5262a26b8f4f4a" +dependencies = [ + "anstyle", + "anstyle-parse", + "anstyle-query", + "anstyle-wincon", + "colorchoice", + "is_terminal_polyfill", + "utf8parse", +] + +[[package]] +name = "anstyle" +version = "1.0.13" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "5192cca8006f1fd4f7237516f40fa183bb07f8fbdfedaa0036de5ea9b0b45e78" + +[[package]] +name = "anstyle-parse" +version = "0.2.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4e7644824f0aa2c7b9384579234ef10eb7efb6a0deb83f9630a49594dd9c15c2" +dependencies = [ + "utf8parse", +] + +[[package]] +name = "anstyle-query" +version = "1.1.5" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "40c48f72fd53cd289104fc64099abca73db4166ad86ea0b4341abe65af83dadc" +dependencies = [ + "windows-sys 0.61.2", +] + +[[package]] +name = "anstyle-wincon" +version = "3.0.11" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "291e6a250ff86cd4a820112fb8898808a366d8f9f58ce16d1f538353ad55747d" +dependencies = [ + "anstyle", + "once_cell_polyfill", + "windows-sys 0.61.2", +] + [[package]] name = "approx" version = "0.5.1" @@ -138,6 +190,46 @@ dependencies = [ "windows-link", ] +[[package]] +name = "clap" +version = "4.5.57" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6899ea499e3fb9305a65d5ebf6e3d2248c5fab291f300ad0a704fbe142eae31a" +dependencies = [ + "clap_builder", + "clap_derive", +] + +[[package]] +name = "clap_builder" +version = "4.5.57" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "7b12c8b680195a62a8364d16b8447b01b6c2c8f9aaf68bee653be34d4245e238" +dependencies = [ + "anstream", + "anstyle", + "clap_lex", + "strsim", +] + +[[package]] +name = "clap_derive" +version = "4.5.55" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a92793da1a46a5f2a02a6f4c46c6496b28c43638adea8306fcb0caa1634f24e5" +dependencies = [ + "heck", + "proc-macro2", + "quote", + "syn", +] + +[[package]] +name = "clap_lex" +version = "0.7.7" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "c3e64b0cc0439b12df2fa678eae89a1c56a529fd067a9115f7827f1fffd22b32" + [[package]] name = "clearscreen" version = "4.0.2" @@ -157,6 +249,12 @@ version = "1.1.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "3d7b894f5411737b7867f4827955924d7c254fc9f4d91a6aad6b097804b1018b" +[[package]] +name = "colorchoice" +version = "1.0.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "b05b61dc5112cbb17e4b6cd61790d9845d13888356391624cbe7e41efeac1e75" + [[package]] name = "core-foundation" version = "0.10.1" @@ -645,6 +743,12 @@ dependencies = [ "mach2", ] +[[package]] +name = "is_terminal_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a6cb138bb79a146c1bd460005623e142ef0181e3d0219cb493e02f7d08a35695" + [[package]] name = "itoa" version = "1.0.17" @@ -941,6 +1045,12 @@ version = "1.21.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "42f5e15c9953c5e4ccceeb2e7382a716482c34515315f7b03532b8b4e8393d2d" +[[package]] +name = "once_cell_polyfill" +version = "1.70.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "384b8ab6d37215f3c5301a95a4accb5d64aa607f1fcb26a11b5303878451b4fe" + [[package]] name = "ordered-float" version = "5.1.0" @@ -1591,6 +1701,12 @@ version = "1.0.22" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "9312f7c4f6ff9069b165498234ce8be658059c6728633667c526e27dc2cf1df5" +[[package]] +name = "utf8parse" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "06abde3611657adf66d383f00b093d7faecc7fa57071cce2578660c9f1010821" + [[package]] name = "uuid" version = "1.19.0" diff --git a/Cargo.toml b/Cargo.toml index 3fa1eab..92c6322 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,6 +18,10 @@ toml = "0.9.8" csv = "1.4.0" serde_with = "3" +serde_json = "1.0.149" + + +clap = { version = "4", features = ["derive"] } # [profile.dev.package.rapier3d] @@ -25,3 +29,7 @@ serde_with = "3" [profile.dev.package."*"] opt-level = 3 + +[[bin]] +name = "record" +path = "src/main_record.rs" diff --git a/configurations/pid_cont/rate_01.toml b/configurations/pid_cont/rate_01.toml index 46688bd..e28c0c3 100644 --- a/configurations/pid_cont/rate_01.toml +++ b/configurations/pid_cont/rate_01.toml @@ -12,5 +12,5 @@ layers = [ 0.0, 0.0, 0.0, - ] }, + ], frequency = 600.0 }, ] diff --git a/configurations/pid_cont/rate_02.toml b/configurations/pid_cont/rate_02.toml index 72b0d15..dbc8086 100644 --- a/configurations/pid_cont/rate_02.toml +++ b/configurations/pid_cont/rate_02.toml @@ -12,5 +12,5 @@ layers = [ 0.0, 0.0, 0.0, - ] }, + ], frequency = 600.0 }, ] diff --git a/configurations/pid_cont/rate_03.toml b/configurations/pid_cont/rate_03.toml index 72b0d15..dbc8086 100644 --- a/configurations/pid_cont/rate_03.toml +++ b/configurations/pid_cont/rate_03.toml @@ -12,5 +12,5 @@ layers = [ 0.0, 0.0, 0.0, - ] }, + ], frequency = 600.0 }, ] diff --git a/inputs/ad.csv b/inputs/ad.csv deleted file mode 100644 index 945b235..0000000 --- a/inputs/ad.csv +++ /dev/null @@ -1,50 +0,0 @@ -time,throttle,yaw,pitch,roll -0,0,0,0,0 -0.78333336,0,-1,0,0 -1.7333333,0,1,0,0 -3.1000001,0,-1,0,0 -5.016667,0,0,0,0 -5.4333334,0,1,0,0 -6.383333,0,0,0,0 -6.6666665,0,-1,0,0 -7.1,0,0,0,0 -7.2333336,0,1,0,0 -7.383333,0,-1,0,0 -7.4500003,0,1,0,0 -7.5833335,0,-1,0,0 -7.65,0,1,0,0 -7.7333336,0,-1,0,0 -7.85,0,1,0,0 -7.9500003,0,-1,0,0 -8.45,0,0,0,0 -8.466667,0,1,0,0 -8.900001,0,0,0,0 -9.3,0,1,0,0 -9.900001,0,-1,0,0 -10.433333,0,0,0,0 -10.883333,0,-1,0,0 -11.35,0,1,0,0 -11.716667,0,-1,0,0 -11.866667,0,1,0,0 -12.05,0,-1,0,0 -12.133333,0,1,0,0 -12.266666,0,-1,0,0 -12.333333,0,1,0,0 -12.433333,0,0,0,0 -12.566667,0,1,0,0 -13.7,0,-1,0,0 -14.533334,0,1,0,0 -15.866667,0,-1,0,0 -17.316668,0,0,0,0 -17.800001,0,1,0,0 -18.333334,0,0,0,0 -18.516666,0,-1,0,0 -18.933334,0,0,0,0 -19.216667,0,1,0,0 -19.7,0,0,0,0 -19.783333,0,-1,0,0 -20.25,0,0,0,0 -20.5,0,1,0,0 -20.95,0,0,0,0 -21.033333,0,-1,0,0 -21.383333,0,0,0,0 diff --git a/inputs/leftright.csv b/inputs/leftright.csv deleted file mode 100644 index 5ccf6ba..0000000 --- a/inputs/leftright.csv +++ /dev/null @@ -1,34 +0,0 @@ -time,throttle,yaw,pitch,roll -0,0,0,0,0 -1.3666667,0,0,0,-1 -1.7,0,0,0,0 -1.7333333,0,0,0,1 -2.4333334,0,0,0,0 -2.45,0,0,0,-1 -2.6833334,0,0,0,1 -3.1833334,0,0,0,-1 -3.8833334,0,0,0,1 -4.133333,0,0,0,-1 -4.3166666,0,0,0,1 -4.4333334,0,0,0,0 -4.4666667,0,0,0,-1 -4.5333333,0,0,0,0 -4.55,0,0,0,1 -4.633333,0,0,0,0 -4.6666665,0,0,0,-1 -4.7333336,0,0,0,0 -4.75,0,0,0,1 -4.9833336,0,0,0,-1 -5.0833335,0,0,0,0 -5.133333,0,0,0,1 -5.2166667,0,0,0,0 -5.3333335,0,0,0,1 -5.4,0,0,0,0 -5.4166665,0,0,0,-1 -5.5333333,0,0,0,0 -5.5666666,0,0,0,1 -5.6,0,0,0,-1 -6.0333333,0,0,0,0 -6.05,0,0,0,1 -7.7166667,0,0,0,-1 -9.216667,0,0,0,0 diff --git a/inputs/test.csv b/inputs/test.csv deleted file mode 100644 index 73557f1..0000000 --- a/inputs/test.csv +++ /dev/null @@ -1,144 +0,0 @@ -time,throttle,yaw,pitch,roll -0,0,0,0,0 -1.0333333,0,0,1,0 -2.55,0,0,0,0 -2.8999999,1,0,0,0 -3.35,0,0,0,0 -3.5833333,0,0,-1,0 -3.7666667,0,0,0,0 -4.0833335,0,0,1,0 -4.366667,0,0,0,0 -4.5499997,0,0,0,-1 -4.6666665,0,0,0,0 -4.7166667,0,0,0,1 -4.8333335,0,0,0,0 -4.85,0,0,0,-1 -4.9166665,0,0,0,0 -4.95,0,0,0,1 -5.0499997,0,0,0,0 -5.116667,0,0,0,-1 -5.1833334,0,0,0,1 -5.25,0,0,0,0 -5.633333,0,0,-1,0 -5.7999997,0,0,0,0 -5.8166666,0,0,1,0 -5.9166665,0,0,0,0 -5.9666667,0,0,-1,0 -6.0833335,0,0,1,0 -6.1833334,0,0,0,0 -6.2,0,0,-1,0 -6.2999997,0,0,0,0 -6.633333,0,-1,0,0 -6.7166667,0,1,0,0 -6.9,0,0,0,0 -6.9166665,0,-1,0,0 -7.0499997,0,1,0,0 -7.2,0,-1,0,0 -7.2999997,0,1,0,0 -7.45,0,-1,0,0 -7.5666666,0,0,0,0 -8.066667,0,0,1,0 -8.233334,0,0,0,0 -8.433333,0,0,0,-1 -8.483334,0,1,0,-1 -8.766666,0,1,1,-1 -9.233334,0,0,0,-1 -9.483334,0,1,0,0 -9.65,0,0,0,0 -9.7,0,0,0,1 -10.3,0,0,0,0 -10.4,1,0,0,0 -10.883333,0,0,0,0 -10.933333,0,0,-1,0 -11.066667,0,0,0,0 -11.316667,0,0,-1,0 -11.483334,0,0,0,0 -11.916667,0,0,-1,0 -12.05,1,0,0,0 -12.266666,1,0,1,0 -12.483334,1,0,0,0 -12.883333,1,0,-1,0 -13.033333,1,0,0,0 -13.366667,1,0,-1,0 -13.483334,1,0,0,0 -14.15,1,0,1,0 -14.166667,1,0,1,1 -14.466666,1,0,0,1 -14.5,1,0,0,0 -14.75,1,-1,0,0 -14.766666,1,-1,1,0 -15.033333,1,-1,1,-1 -15.233334,1,-1,1,0 -15.4,1,-1,0,0 -15.483334,1,0,0,0 -15.516666,0,0,0,0 -15.716666,0,0,-1,0 -16.133333,0,0,0,0 -16.716667,0,0,0,-1 -16.8,1,0,0,-1 -16.916666,1,0,0,0 -17.216667,1,0,-1,0 -17.35,1,0,0,0 -17.516666,1,0,0,1 -17.533333,0,0,0,1 -17.7,0,0,0,0 -17.983334,0,0,0,-1 -18.216667,0,0,0,0 -18.316666,0,-1,0,0 -18.516666,0,0,0,0 -18.6,0,0,0,1 -18.766666,0,0,0,0 -19.449999,1,0,0,0 -19.833334,1,0,-1,0 -19.916666,1,0,0,0 -21.866667,1,-1,0,0 -21.9,1,-1,0,1 -22.066666,1,-1,1,1 -22.533333,1,-1,0,0 -22.583334,0,-1,0,0 -22.6,0,-1,0,-1 -22.683332,0,0,0,-1 -22.966667,0,0,0,0 -23.066666,0,1,0,0 -23.216667,0,0,0,0 -23.716667,0,0,0,-1 -23.833334,0,0,0,0 -26.016666,1,0,0,0 -27.116667,1,1,0,0 -27.433332,1,1,0,-1 -28.516666,1,0,0,-1 -28.533333,1,-1,0,-1 -28.566666,1,-1,0,1 -29.65,1,-1,0,0 -29.683332,1,-1,-1,0 -30.083332,1,-1,0,0 -30.1,1,-1,1,0 -30.666666,0,0,1,0 -30.699999,0,0,0,0 -30.716667,0,1,0,0 -31,0,1,0,-1 -32.316666,0,0,0,-1 -32.416668,0,0,0,0 -32.483334,0,0,1,0 -32.966667,0,0,0,0 -33.066666,0,0,1,0 -33.416668,0,0,0,0 -33.466667,0,-1,0,0 -33.483334,0,-1,0,1 -33.833332,0,0,0,1 -34.116665,0,0,0,0 -34.183334,1,0,0,0 -34.3,1,0,-1,0 -34.566666,1,0,0,-1 -34.716667,1,0,0,0 -35.216667,1,0,-1,0 -35.566666,1,0,-1,-1 -35.616665,1,0,0,-1 -35.75,1,0,0,0 -36.016666,1,0,1,0 -36.183334,1,0,0,0 -36.216667,0,0,0,0 -36.983334,1,0,0,0 -37.066666,1,0,1,0 -37.399998,1,0,0,0 -37.483334,0,0,0,0 diff --git a/inputs/updown.csv b/inputs/updown.csv deleted file mode 100644 index 8380b64..0000000 --- a/inputs/updown.csv +++ /dev/null @@ -1,37 +0,0 @@ -time,throttle,yaw,pitch,roll -0,0,0,0,0 -1.05,0,0,-1,0 -1.6333333,0,0,0,0 -1.65,0,0,1,0 -2.1,0,0,0,0 -2.1333334,0,0,-1,0 -2.4666667,0,0,0,0 -2.4833333,0,0,1,0 -2.7833333,0,0,0,0 -2.8,0,0,-1,0 -5.1666665,0,0,0,0 -5.2000003,0,0,1,0 -7.5333333,0,0,0,0 -7.5666666,0,0,-1,0 -7.7333336,0,0,0,0 -7.75,0,0,1,0 -7.916667,0,0,-1,0 -8.033334,0,0,0,0 -8.05,0,0,1,0 -8.15,0,0,0,0 -8.166667,0,0,-1,0 -8.25,0,0,0,0 -8.283334,0,0,1,0 -8.383333,0,0,-1,0 -8.466667,0,0,1,0 -8.6,0,0,-1,0 -8.733334,0,0,1,0 -8.85,0,0,0,0 -8.883333,0,0,-1,0 -8.966667,0,0,1,0 -9.05,0,0,0,0 -9.066667,0,0,-1,0 -9.583333,0,0,1,0 -10.616667,0,0,-1,0 -11.466667,0,0,1,0 -11.933333,0,0,0,0 diff --git a/src/config.rs b/src/config.rs index 767515b..d5d370b 100644 --- a/src/config.rs +++ b/src/config.rs @@ -16,12 +16,14 @@ pub enum LayerConfig { #[serde(flatten)] pid: PidConfig, max_rate: f32, + frequency: f32, }, /// Controls orientation (Input: joystick -> Output: desired angular velocity) Angle { #[serde(flatten)] pid: PidConfig, max_angle: f32, + frequency: f32, }, } diff --git a/src/drone/input.rs b/src/drone/input.rs index 6e01608..a79d1df 100644 --- a/src/drone/input.rs +++ b/src/drone/input.rs @@ -1,16 +1,132 @@ #![allow(unused)] use macroquad::prelude as mq; +use serde::{Deserialize, Serialize}; +use std::fs::File; +use std::io::{Read, Write}; -#[derive(Default, Clone, Copy, PartialEq)] +#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)] pub struct JoystickInput { - // Value should be between 0 and 1 pub throttle_input: f32, - pub roll_input: f32, pub yaw_input: f32, pub pitch_input: f32, } +#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)] +pub struct PositionInput { + pub lat: f32, + pub long: f32, + pub alt: f32, +} + +#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)] +pub enum ModeInput { + #[default] + Acro, + Navigation, +} + +#[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)] +pub struct Input { + pub joystick: JoystickInput, + pub position: PositionInput, + pub mode: ModeInput, +} + +#[derive(Default, Clone, Copy, serde::Serialize, serde::Deserialize)] +pub struct InputRecord { + input: Input, + time: f32, +} + +#[derive(Default, Clone, serde::Serialize, serde::Deserialize)] +pub struct InputRecording { + records: Vec, +} + +impl InputRecording { + pub fn save_to_file(&self, path: &str) -> Result<(), Box> { + let json = serde_json::to_string_pretty(self)?; + let mut file = File::create(path)?; + file.write_all(json.as_bytes())?; + Ok(()) + } + + pub fn load_from_file(path: &str) -> Result> { + let mut file = File::open(path)?; + let mut contents = String::new(); + file.read_to_string(&mut contents)?; + let recording: Self = serde_json::from_str(&contents)?; + Ok(recording) + } + + pub fn has_ended(&self, time: f32) -> bool { + return self.records.last().unwrap_or(&InputRecord::default()).time < time; + } + pub fn get_input(&self, time: f32) -> Input { + /* + * Binary search returns index to element as OK, or where the element could be placed to + * keep order as Err, so if result is Ok return that input, if its Err, return the previous + * input if it exists, if it doesn't (because time is before the first action in the + * recorded sequence, return an empty input) + */ + let res = self + .records + .binary_search_by(|probe| probe.time.total_cmp(&time)); + match res { + Ok(res) => { + return self + .records + .get(res) + .unwrap_or(&InputRecord::default()) + .input; + } + Err(mut res) => { + if res > 0 { + res -= 1; + } + return self + .records + .get(0.max(res)) + .unwrap_or(&InputRecord::default()) + .input; + } + } + } + + /* + * Current time should always be larger thant the last input records time. + * This method is made for recording inputs in real time, not for retroactively adding + * + * Returns the addded Joystick Input + */ + pub fn add_input_from_keyboard(&mut self, current_time: f32) -> Input { + let input = Input { + joystick: JoystickInput::from_keyboard(), + position: Default::default(), + mode: ModeInput::Acro, + }; + let last_input = self.records.last(); + match last_input { + Some(last_record) => { + if last_record.input != input { + self.records.push(InputRecord { + input, + time: current_time, + }); + } + } + None => { + self.records.push(InputRecord { + input, + time: current_time, + }); + } + } + return input; + } +} + impl JoystickInput { pub fn from_keyboard() -> Self { let mut input = Self::default(); @@ -48,173 +164,3 @@ impl JoystickInput { return input; } } - -#[derive(Default, Clone, Copy)] -pub struct InputRecord { - pub input: JoystickInput, - pub timestamp: f32, -} - -impl InputRecord { - pub fn record_from_keyboard(timestamp: f32) -> Self { - Self { - input: JoystickInput::from_keyboard(), - timestamp, - } - } -} - -#[derive(Default, Clone)] -pub struct InputRecording { - pub input_records: Vec, -} - -impl InputRecording { - pub fn new() -> Self { - Self { - input_records: Vec::new(), - } - } - pub fn save_inputs_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,throttle,yaw,pitch,roll")?; - - for record in self.input_records.iter() { - let inp = record.input; - writeln!( - file, - "{},{},{},{},{}", - record.timestamp, - inp.throttle_input, - inp.yaw_input, - inp.pitch_input, - inp.roll_input - )?; - } - - Ok(()) - } - - pub fn load_inputs_from_csv(path: &str) -> std::io::Result { - use std::fs::File; - use std::io::{BufRead, BufReader}; - - let file = File::open(path)?; - let reader = BufReader::new(file); - - let mut input_records = Vec::new(); - - for (i, line) in reader.lines().enumerate() { - let line = line?; - if i == 0 { - continue; - } - - let parts: Vec<&str> = line.split(',').collect(); - if parts.len() != 5 { - continue; // Should return an error to be strict - } - - let timestamp: f32 = parts[0] - .parse() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?; - - let throttle: f32 = parts[1] - .parse() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?; - let yaw: f32 = parts[2] - .parse() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?; - let pitch: f32 = parts[3] - .parse() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?; - let roll: f32 = parts[4] - .parse() - .map_err(|e| std::io::Error::new(std::io::ErrorKind::InvalidData, e))?; - - let input = JoystickInput { - throttle_input: throttle, - yaw_input: yaw, - pitch_input: pitch, - roll_input: roll, - }; - - input_records.push(InputRecord { input, timestamp }); - } - - Ok(Self { input_records }) - } - - pub fn get_input(&self, time: f32) -> JoystickInput { - /* - * Binary search returns index to element as OK, or where the element could be placed to - * keep order as Err, so if result is Ok return that input, if its Err, return the previous - * input if it exists, if it doesn't (because time is before the first action in the - * recorded sequence, return an empty input) - */ - let res = self - .input_records - .binary_search_by(|probe| probe.timestamp.total_cmp(&time)); - match res { - Ok(res) => { - return self - .input_records - .get(res) - .unwrap_or(&InputRecord::default()) - .input; - } - Err(mut res) => { - if res > 0 { - res -= 1; - } - return self - .input_records - .get(0.max(res)) - .unwrap_or(&InputRecord::default()) - .input; - } - } - } - - pub fn ended(&self, time: f32) -> bool { - match self.input_records.last() { - Some(record) => { - return record.timestamp < time; - } - None => { - return true; - } - } - } - - /* - * Current time should always be larger thant the last input records time. - * This method is made for recording inputs in real time, not for retroactively adding - * - * Returns the addded Joystick Input - */ - pub fn add_input_from_keyboard(&mut self, current_time: f32) -> JoystickInput { - let input = JoystickInput::from_keyboard(); - let last_input = self.input_records.last(); - match last_input { - Some(last_record) => { - if last_record.input != input { - self.input_records.push(InputRecord { - input, - timestamp: current_time, - }); - } - } - None => { - self.input_records.push(InputRecord { - input, - timestamp: current_time, - }); - } - } - return input; - } -} diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs index 80ff152..9fc55c1 100644 --- a/src/drone/stacked.rs +++ b/src/drone/stacked.rs @@ -3,8 +3,9 @@ use nalgebra::{self as na, vector}; use std::{any::Any, f32}; -use crate::drone::controller::DroneController; +use crate::drone::stacked::modules::ModuleRuntime; use crate::drone::JoystickInput; +use crate::drone::{controller::DroneController, input::Input}; use crate::config::*; @@ -20,39 +21,56 @@ pub struct DroneState { } pub struct StackedController { - modules: Vec, + modules: Vec, config: SimulationConfig, mixer: MotorMixer, // State drone_state: DroneState, - input: JoystickInput, + input: Input, last_time: f32, current_time: f32, } impl StackedController { - pub fn set_input(&mut self, inp: JoystickInput) { + pub fn set_input(&mut self, inp: Input) { 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 { + let (module, freq) = match layer { + LayerConfig::Angle { + pid, + max_angle, + frequency, + } => ( + ControllerModule::Angle { processor: PidProcessor::new(pid), max_angle: *max_angle, - }); - } - LayerConfig::Rate { pid, max_rate } => { - modules.push(ControllerModule::Rate { + }, + *frequency, + ), + LayerConfig::Rate { + pid, + max_rate, + frequency, + } => ( + ControllerModule::Rate { processor: PidProcessor::new(pid), max_rate: *max_rate, - }); - } - } + }, + *frequency, + ), + }; + + modules.push(ModuleRuntime { + module, + target_dt: 1.0 / freq, // Convert Hz to Seconds + accumulated_time: 0.0, + last_output: na::Vector3::zeros(), + }); } Self { @@ -62,9 +80,9 @@ impl StackedController { max_throttle: 1.0, mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 }, }, - modules, + modules, // Now Vec config, - input: JoystickInput::default(), + input: Input::default(), drone_state: DroneState { rotation: na::UnitQuaternion::identity(), angular_velocity: na::Vector3::zeros(), @@ -88,20 +106,39 @@ impl DroneController for StackedController { } fn get_motor_throttles(&mut self) -> [f32; 4] { - let dt = (self.current_time - self.last_time).max(0.0001); + let frame_dt = (self.current_time - self.last_time).max(0.0); - let mut setpoint = vector![ - self.input.roll_input, - self.input.yaw_input, - self.input.pitch_input, + // Initial setpoint comes from the sticks + let mut current_setpoint = vector![ + self.input.joystick.roll_input, + self.input.joystick.yaw_input, + self.input.joystick.pitch_input, ]; - 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); + for (i, runtime) in self.modules.iter_mut().enumerate() { + runtime.accumulated_time += frame_dt; + + if runtime.accumulated_time >= runtime.target_dt { + // is_first_layer logic: only true if it's the very first module in the vec + let is_first_layer = i == 0; + + runtime.last_output = runtime.module.process( + current_setpoint, + &self.drone_state, + runtime.accumulated_time, + is_first_layer, + ); + + // Subtract to keep timing consistent and account for frame jitter + runtime.accumulated_time -= runtime.target_dt; + } + + // Pass this module's output (either fresh or cached) to the next module + current_setpoint = runtime.last_output; } - return self.mixer.mix(0.5, setpoint); + self.mixer + .mix(self.input.joystick.throttle_input, current_setpoint) } fn as_any(&self) -> &dyn Any { diff --git a/src/drone/stacked/modules.rs b/src/drone/stacked/modules.rs index 0477cb1..52e41c5 100644 --- a/src/drone/stacked/modules.rs +++ b/src/drone/stacked/modules.rs @@ -13,6 +13,13 @@ pub enum ControllerModule { }, } +pub struct ModuleRuntime { + pub module: ControllerModule, + pub target_dt: f32, // e.g., 0.01 for 100Hz + pub accumulated_time: f32, + pub last_output: na::Vector3, // Store the last result to pass down the chain +} + impl ControllerModule { pub fn process( &mut self, @@ -27,7 +34,11 @@ impl ControllerModule { max_angle, } => { // Setpoint is -1.0..1.0, scale it to target Radians - let target_angles = setpoint * *max_angle; + let target_angles = if is_first_layer { + setpoint * *max_angle + } else { + setpoint + }; let (r, p, y) = state.rotation.euler_angles(); let current_angles = na::vector![r, y, p]; diff --git a/src/main.rs b/src/main.rs index c389583..cf12cab 100644 --- a/src/main.rs +++ b/src/main.rs @@ -56,7 +56,7 @@ fn run_batch() { 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()) + let inputs = InputRecording::load_from_file(input_path.to_str().unwrap()) .expect("Failed to load input recording"); let config_name = config_path.file_stem().unwrap().to_string_lossy(); diff --git a/src/main_record.rs b/src/main_record.rs new file mode 100644 index 0000000..a38f08f --- /dev/null +++ b/src/main_record.rs @@ -0,0 +1,95 @@ +mod engine; +use engine::*; +mod camera; +mod config; +mod drone; +mod helpers; +mod logger; +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/final/"; +const RESULTS_DIR: &str = "results/"; + +use clap::Parser; +use std::path::PathBuf; +use std::thread::JoinHandle; + +use macroquad::prelude as mq; + +fn window_conf() -> mq::Conf { + mq::Conf { + window_title: "RustDroneSim".to_owned(), + window_resizable: true, + // fullscreen: true, + platform: mq::miniquad::conf::Platform { + // linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly, + ..Default::default() + }, + ..Default::default() + } +} + +#[derive(Parser)] +struct Args { + input: PathBuf, + config: PathBuf, +} + +#[macroquad::main(window_conf)] +async fn main() { + let args = Args::parse(); + // run_batch(); + run_record( + args.input.to_string_lossy().to_string(), + &args.config, + // "inputs/newtest.json".to_string(), + // &PathBuf::from("configurations/final/sim_std_mot_std_rate_01.toml".to_string()), + ) + .await; +} + +async fn run_record(output: String, config_path: &PathBuf) { + println!( + "Recording inputs to {}, using config {}", + output, + config_path.to_str().unwrap_or("") + ); + + let config: SimulationConfig = + toml::from_str(&fs::read_to_string(config_path).unwrap()).expect("Invalid config file"); + + 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 mut sim = Simulation::new( + drone, + world, + SimMode::Record(InputRecording::default(), output), + None, + config.drone_tick_rate, + ); + + sim.run_and_render().await.unwrap(); +} diff --git a/src/simulation.rs b/src/simulation.rs index 2fed0a1..e7fbb29 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -6,7 +6,7 @@ use std::error::Error; use crate::{ drone::{ controller::DroneController, - input::{InputRecording, JoystickInput}, + input::{Input, InputRecording}, Drone, }, engine::World, @@ -120,7 +120,7 @@ impl Simulation { } fn step(&mut self) -> Result> { - let current_input: JoystickInput; + let current_input: Input; let current_time = self.world.get_time() as f32; match &mut self.mode { @@ -134,7 +134,7 @@ impl Simulation { let playback_time = current_time - *start_time; current_input = recording.get_input(playback_time); - if recording.ended(playback_time) { + if recording.has_ended(playback_time) { println!("Playback ended."); return Ok(StepOutcome::Exit); } @@ -160,9 +160,9 @@ impl Simulation { self.drone.process_tick(&mut self.world); let target_angular_vel: na::Vector3 = na::vector![ - current_input.roll_input, - current_input.yaw_input, - current_input.pitch_input, + current_input.joystick.roll_input, + current_input.joystick.yaw_input, + current_input.joystick.pitch_input, ] * 3.14; let applied_motor_diff: na::Vector3 = na::vector![ @@ -198,7 +198,7 @@ impl Simulation { fn shutdown(&mut self) -> Result<(), Box> { // Save input recording if needed if let SimMode::Record(recording, dest) = &self.mode { - recording.save_inputs_to_csv(dest)?; + recording.save_to_file(dest)?; println!("Input recording saved to {}", dest); } if let Some(logger) = &mut self.logger {