idk this is math class
This commit is contained in:
franchioping
parent
e776c641b5
commit
6f61d4ab43
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -12,5 +12,5 @@ layers = [
|
|||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
] },
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
|
|
|
|||
|
|
@ -12,5 +12,5 @@ layers = [
|
|||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
] },
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
|
|
|
|||
|
|
@ -12,5 +12,5 @@ layers = [
|
|||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
] },
|
||||
], frequency = 600.0 },
|
||||
]
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
144
inputs/test.csv
144
inputs/test.csv
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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,
|
||||
},
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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<InputRecord>,
|
||||
}
|
||||
|
||||
impl InputRecording {
|
||||
pub fn save_to_file(&self, path: &str) -> Result<(), Box<dyn std::error::Error>> {
|
||||
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<Self, Box<dyn std::error::Error>> {
|
||||
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<InputRecord>,
|
||||
}
|
||||
|
||||
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<Self> {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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,40 +21,57 @@ pub struct DroneState {
|
|||
}
|
||||
|
||||
pub struct StackedController {
|
||||
modules: Vec<ControllerModule>,
|
||||
modules: Vec<ModuleRuntime>,
|
||||
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 {
|
||||
mixer: MotorMixer {
|
||||
|
|
@ -62,9 +80,9 @@ impl StackedController {
|
|||
max_throttle: 1.0,
|
||||
mixing_mode: mixer::MotorMixingMode::ThrottleAuthorityReasonable { min_scale: 0.5 },
|
||||
},
|
||||
modules,
|
||||
modules, // Now Vec<ModuleRuntime>
|
||||
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() {
|
||||
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;
|
||||
setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
return self.mixer.mix(0.5, setpoint);
|
||||
// Pass this module's output (either fresh or cached) to the next module
|
||||
current_setpoint = runtime.last_output;
|
||||
}
|
||||
|
||||
self.mixer
|
||||
.mix(self.input.joystick.throttle_input, current_setpoint)
|
||||
}
|
||||
|
||||
fn as_any(&self) -> &dyn Any {
|
||||
|
|
|
|||
|
|
@ -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<f32>, // 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];
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
|
|
@ -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("<INVALID CONFIG PATH>")
|
||||
);
|
||||
|
||||
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();
|
||||
}
|
||||
|
|
@ -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<StepOutcome, Box<dyn Error>> {
|
||||
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<f32> = 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<f32> = na::vector![
|
||||
|
|
@ -198,7 +198,7 @@ impl Simulation {
|
|||
fn shutdown(&mut self) -> Result<(), Box<dyn Error>> {
|
||||
// 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 {
|
||||
|
|
|
|||
Loading…
Reference in New Issue