working code finally? batching multiple simulations + recoding input
This commit is contained in:
parent
1f572dc0e5
commit
6ad1488070
|
|
@ -1 +1,2 @@
|
||||||
/target
|
/target
|
||||||
|
results/
|
||||||
|
|
|
||||||
|
|
@ -13,7 +13,9 @@ dependencies = [
|
||||||
"nalgebra",
|
"nalgebra",
|
||||||
"rand 0.9.2",
|
"rand 0.9.2",
|
||||||
"rapier3d",
|
"rapier3d",
|
||||||
|
"serde",
|
||||||
"strum",
|
"strum",
|
||||||
|
"toml",
|
||||||
]
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
|
|
@ -431,6 +433,16 @@ dependencies = [
|
||||||
"png",
|
"png",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "indexmap"
|
||||||
|
version = "2.12.1"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "0ad4bb2b565bca0645f4d68c5c9af97fba094e9791da685bf83cb5f3ce74acf2"
|
||||||
|
dependencies = [
|
||||||
|
"equivalent",
|
||||||
|
"hashbrown 0.16.1",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "inotify"
|
name = "inotify"
|
||||||
version = "0.11.0"
|
version = "0.11.0"
|
||||||
|
|
@ -1031,6 +1043,45 @@ dependencies = [
|
||||||
"bytemuck",
|
"bytemuck",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "serde"
|
||||||
|
version = "1.0.228"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "9a8e94ea7f378bd32cbbd37198a4a91436180c5bb472411e48b5ec2e2124ae9e"
|
||||||
|
dependencies = [
|
||||||
|
"serde_core",
|
||||||
|
"serde_derive",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "serde_core"
|
||||||
|
version = "1.0.228"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "41d385c7d4ca58e59fc732af25c3983b67ac852c1a25000afe1175de458b67ad"
|
||||||
|
dependencies = [
|
||||||
|
"serde_derive",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "serde_derive"
|
||||||
|
version = "1.0.228"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "d540f220d3187173da220f885ab66608367b6574e925011a9353e4badda91d79"
|
||||||
|
dependencies = [
|
||||||
|
"proc-macro2",
|
||||||
|
"quote",
|
||||||
|
"syn",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "serde_spanned"
|
||||||
|
version = "1.0.3"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "e24345aa0fe688594e73770a5f6d1b216508b4f93484c0026d521acd30134392"
|
||||||
|
dependencies = [
|
||||||
|
"serde_core",
|
||||||
|
]
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "simba"
|
name = "simba"
|
||||||
version = "0.9.1"
|
version = "0.9.1"
|
||||||
|
|
@ -1156,6 +1207,45 @@ dependencies = [
|
||||||
"syn",
|
"syn",
|
||||||
]
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "toml"
|
||||||
|
version = "0.9.8"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f0dc8b1fb61449e27716ec0e1bdf0f6b8f3e8f6b05391e8497b8b6d7804ea6d8"
|
||||||
|
dependencies = [
|
||||||
|
"indexmap",
|
||||||
|
"serde_core",
|
||||||
|
"serde_spanned",
|
||||||
|
"toml_datetime",
|
||||||
|
"toml_parser",
|
||||||
|
"toml_writer",
|
||||||
|
"winnow",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "toml_datetime"
|
||||||
|
version = "0.7.3"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "f2cdb639ebbc97961c51720f858597f7f24c4fc295327923af55b74c3c724533"
|
||||||
|
dependencies = [
|
||||||
|
"serde_core",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "toml_parser"
|
||||||
|
version = "1.0.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "c0cbe268d35bdb4bb5a56a2de88d0ad0eb70af5384a99d648cd4b3d04039800e"
|
||||||
|
dependencies = [
|
||||||
|
"winnow",
|
||||||
|
]
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "toml_writer"
|
||||||
|
version = "1.0.4"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "df8b2b54733674ad286d16267dcfc7a71ed5c776e4ac7aa3c3e2561f7c637bf2"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "ttf-parser"
|
name = "ttf-parser"
|
||||||
version = "0.21.1"
|
version = "0.21.1"
|
||||||
|
|
@ -1489,6 +1579,12 @@ version = "0.52.6"
|
||||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
|
checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
|
||||||
|
|
||||||
|
[[package]]
|
||||||
|
name = "winnow"
|
||||||
|
version = "0.7.14"
|
||||||
|
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||||
|
checksum = "5a5364e9d77fcdeeaa6062ced926ee3381faa2ee02d3eb83a5c27a8825540829"
|
||||||
|
|
||||||
[[package]]
|
[[package]]
|
||||||
name = "winsafe"
|
name = "winsafe"
|
||||||
version = "0.0.19"
|
version = "0.0.19"
|
||||||
|
|
|
||||||
|
|
@ -13,8 +13,13 @@ rand = "0.9.2"
|
||||||
strum = { version = "0.27", features = ["derive"] }
|
strum = { version = "0.27", features = ["derive"] }
|
||||||
clearscreen = "4.0.2"
|
clearscreen = "4.0.2"
|
||||||
gilrs = "0.11.0"
|
gilrs = "0.11.0"
|
||||||
|
serde = {version = "1.0.228", features = ["serde_derive"]}
|
||||||
|
toml = "0.9.8"
|
||||||
|
|
||||||
|
|
||||||
[[bin]]
|
[[bin]]
|
||||||
name = "tools"
|
name = "tools"
|
||||||
path = "src/tools/main.rs"
|
path = "src/tools/main.rs"
|
||||||
|
|
||||||
|
[profile.dev.package.rapier3d]
|
||||||
|
opt-level = 3
|
||||||
|
|
|
||||||
50
analyze.py
50
analyze.py
|
|
@ -2,33 +2,47 @@ import pandas as pd
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
# Load the CSV exported by the PID controller
|
# Load the CSV exported by the PID controller
|
||||||
df = pd.read_csv("pid_logs.csv")
|
df = pd.read_csv("data.csv")
|
||||||
|
|
||||||
# --- Plot target vs current ---
|
# --- Plot target vs current ---
|
||||||
for coord in ["x", "y", "z"]:
|
for coord in ["x", "y", "z"]:
|
||||||
plt.figure()
|
plt.figure()
|
||||||
plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
|
plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
|
||||||
plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
|
plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
|
||||||
|
plt.plot(df["time"], df["mot_" + coord].clip(-1,1), label="mot_" + coord)
|
||||||
|
plt.plot(df["time"], df["dmot_" + coord].clip(-1,1), label="desired mot_" + coord)
|
||||||
plt.xlabel("Time (s)")
|
plt.xlabel("Time (s)")
|
||||||
plt.ylabel("Angular velocity")
|
plt.ylabel("Angular velocity & Motor Offset")
|
||||||
plt.title(f"Target vs Current Angular Velocity ({coord})")
|
plt.title(f"Target vs Current Angular Velocity with Motor Diff ({coord})")
|
||||||
plt.legend()
|
plt.legend(loc='upper right')
|
||||||
plt.grid(True)
|
plt.grid(True)
|
||||||
plt.show()
|
plt.show()
|
||||||
|
|
||||||
plt.figure()
|
# for coord in ["x", "y", "z"]:
|
||||||
plt.plot(df["time"], df["target_x"], label="target_x")
|
# plt.figure()
|
||||||
plt.plot(df["time"], df["current_x"], label="current_x")
|
# plt.plot(df["time"], df["target_" + coord], label="target_" + coord)
|
||||||
plt.plot(df["time"], df["target_y"], label="target_y")
|
# plt.plot(df["time"], df["current_" + coord], label="current_" + coord)
|
||||||
plt.plot(df["time"], df["current_y"], label="current_y")
|
# plt.xlabel("Time (s)")
|
||||||
plt.plot(df["time"], df["target_z"], label="target_z")
|
# plt.ylabel("Angular velocity")
|
||||||
plt.plot(df["time"], df["current_z"], label="current_z")
|
# plt.title(f"Target vs Current Angular Velocity ({coord})")
|
||||||
plt.xlabel("Time (s)")
|
# plt.legend()
|
||||||
plt.ylabel("Angular velocity")
|
# plt.grid(True)
|
||||||
plt.title("Target vs Current Angular Velocity")
|
# plt.show()
|
||||||
plt.legend()
|
|
||||||
plt.grid(True)
|
|
||||||
plt.show()
|
# plt.figure()
|
||||||
|
# plt.plot(df["time"], df["target_x"], label="target_x")
|
||||||
|
# plt.plot(df["time"], df["current_x"], label="current_x")
|
||||||
|
# plt.plot(df["time"], df["target_y"], label="target_y")
|
||||||
|
# plt.plot(df["time"], df["current_y"], label="current_y")
|
||||||
|
# plt.plot(df["time"], df["target_z"], label="target_z")
|
||||||
|
# plt.plot(df["time"], df["current_z"], label="current_z")
|
||||||
|
# plt.xlabel("Time (s)")
|
||||||
|
# plt.ylabel("Angular velocity")
|
||||||
|
# plt.title("Target vs Current Angular Velocity")
|
||||||
|
# plt.legend()
|
||||||
|
# plt.grid(True)
|
||||||
|
# plt.show()
|
||||||
# --- Plot error over time ---
|
# --- Plot error over time ---
|
||||||
plt.figure()
|
plt.figure()
|
||||||
plt.plot(df["time"], df["error_x"], label="error_x")
|
plt.plot(df["time"], df["error_x"], label="error_x")
|
||||||
|
|
@ -37,7 +51,7 @@ plt.plot(df["time"], df["error_z"], label="error_z")
|
||||||
plt.xlabel("Time (s)")
|
plt.xlabel("Time (s)")
|
||||||
plt.ylabel("Error")
|
plt.ylabel("Error")
|
||||||
plt.title("PID Error Over Time")
|
plt.title("PID Error Over Time")
|
||||||
plt.legend()
|
plt.legend(loc='upper right')
|
||||||
plt.grid(True)
|
plt.grid(True)
|
||||||
plt.show()
|
plt.show()
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,13 @@
|
||||||
|
tickrate = 60000.0
|
||||||
|
drone_tick_rate = 600
|
||||||
|
|
||||||
|
target_rate = 3.141592
|
||||||
|
|
||||||
|
proportional_multiplier = [0.05, 0.5, 0.05]
|
||||||
|
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
|
||||||
|
|
||||||
|
|
@ -0,0 +1,144 @@
|
||||||
|
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
|
||||||
|
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
4921
pid_logs.csv
4921
pid_logs.csv
File diff suppressed because it is too large
Load Diff
|
|
@ -1,3 +1,5 @@
|
||||||
|
#![allow(dead_code)]
|
||||||
|
|
||||||
use macroquad::prelude::*;
|
use macroquad::prelude::*;
|
||||||
use std::any::Any;
|
use std::any::Any;
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,46 @@
|
||||||
|
use rapier3d::prelude::*;
|
||||||
|
|
||||||
|
#[derive(Debug, serde::Deserialize)]
|
||||||
|
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],
|
||||||
|
|
||||||
|
// Motors
|
||||||
|
pub max_thrust: f32,
|
||||||
|
pub max_torque: f32,
|
||||||
|
|
||||||
|
// Drone
|
||||||
|
pub mass: f32,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl SimulationConfig {
|
||||||
|
pub fn proportional(&self) -> Vector<f32> {
|
||||||
|
vector![
|
||||||
|
self.proportional_multiplier[0],
|
||||||
|
self.proportional_multiplier[1],
|
||||||
|
self.proportional_multiplier[2]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn integral(&self) -> Vector<f32> {
|
||||||
|
vector![
|
||||||
|
self.integral_multiplier[0],
|
||||||
|
self.integral_multiplier[1],
|
||||||
|
self.integral_multiplier[2]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
|
||||||
|
pub fn diferential(&self) -> Vector<f32> {
|
||||||
|
vector![
|
||||||
|
self.diferential_multiplier[0],
|
||||||
|
self.diferential_multiplier[1],
|
||||||
|
self.diferential_multiplier[2]
|
||||||
|
]
|
||||||
|
}
|
||||||
|
}
|
||||||
15
src/drone.rs
15
src/drone.rs
|
|
@ -5,8 +5,10 @@ use crate::engine::{ColliderExtraData, World};
|
||||||
|
|
||||||
pub mod controller;
|
pub mod controller;
|
||||||
pub mod fpvcontroller;
|
pub mod fpvcontroller;
|
||||||
|
pub mod input;
|
||||||
pub mod pidcontroller;
|
pub mod pidcontroller;
|
||||||
use controller::*;
|
use controller::*;
|
||||||
|
pub use input::JoystickInput;
|
||||||
|
|
||||||
const AIR_DENSITY: f32 = 1.23;
|
const AIR_DENSITY: f32 = 1.23;
|
||||||
const DRAG_CONSTANT: f32 = 1.3;
|
const DRAG_CONSTANT: f32 = 1.3;
|
||||||
|
|
@ -37,8 +39,8 @@ impl Default for MotorCharacteristics {
|
||||||
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: 10.0,
|
max_thrust: 2.6,
|
||||||
max_torque: 1.0,
|
max_torque: 0.5,
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
@ -64,7 +66,7 @@ impl Drone {
|
||||||
) -> Drone {
|
) -> Drone {
|
||||||
let drone_rb_handle = world.register_body(
|
let drone_rb_handle = world.register_body(
|
||||||
rp::RigidBodyBuilder::dynamic()
|
rp::RigidBodyBuilder::dynamic()
|
||||||
.translation(rp::vector![0.0, 10.0, 0.0])
|
.translation(rp::vector![0.0, 100.0, 0.0])
|
||||||
.rotation(rp::vector![0.0, 0.0, 0.0])
|
.rotation(rp::vector![0.0, 0.0, 0.0])
|
||||||
/*
|
/*
|
||||||
* These damping values keep the simulation more realistic,
|
* These damping values keep the simulation more realistic,
|
||||||
|
|
@ -173,6 +175,13 @@ impl Drone {
|
||||||
self.controller.set_rotation(*rb.rotation());
|
self.controller.set_rotation(*rb.rotation());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
pub fn get_angvel(&self, world: &World) -> na::Vector3<f32> {
|
||||||
|
*world.bodies.get(self.rb_handle).unwrap().angvel()
|
||||||
|
}
|
||||||
|
pub fn get_rot(&self, world: &World) -> na::Unit<na::Quaternion<f32>> {
|
||||||
|
*world.bodies.get(self.rb_handle).unwrap().rotation()
|
||||||
|
}
|
||||||
|
|
||||||
pub fn process_tick(&mut self, world: &mut World) {
|
pub fn process_tick(&mut self, world: &mut World) {
|
||||||
self.update_controller(world);
|
self.update_controller(world);
|
||||||
self.apply_throttles(world);
|
self.apply_throttles(world);
|
||||||
|
|
|
||||||
|
|
@ -1,22 +1,9 @@
|
||||||
use rapier3d::prelude as rp;
|
#![allow(dead_code)]
|
||||||
use std::any::Any;
|
use std::any::Any;
|
||||||
|
|
||||||
|
use crate::drone::JoystickInput;
|
||||||
use crate::drone::MotorCharacteristics;
|
use crate::drone::MotorCharacteristics;
|
||||||
|
|
||||||
#[derive(Default, Clone, Copy)]
|
|
||||||
pub struct JoystickInput {
|
|
||||||
// Value should be between 0 and 1
|
|
||||||
pub throttle_input: f32,
|
|
||||||
|
|
||||||
// Rotation Directions: https://upload.wikimedia.org/wikipedia/commons/c/c1/Yaw_Axis_Corrected.svg
|
|
||||||
/*
|
|
||||||
* Values should be between -1 and 1.
|
|
||||||
*/
|
|
||||||
pub yaw_input: f32,
|
|
||||||
pub pitch_input: f32,
|
|
||||||
pub roll_input: f32,
|
|
||||||
}
|
|
||||||
|
|
||||||
impl JoystickInput {
|
impl JoystickInput {
|
||||||
pub fn clamp(&self) -> JoystickInput {
|
pub fn clamp(&self) -> JoystickInput {
|
||||||
return JoystickInput {
|
return JoystickInput {
|
||||||
|
|
|
||||||
|
|
@ -1,9 +1,9 @@
|
||||||
use rapier3d::prelude as rp;
|
#![allow(dead_code)]
|
||||||
use std::any::Any;
|
use std::any::Any;
|
||||||
|
|
||||||
use crate::drone::controller::DroneController;
|
use crate::drone::controller::DroneController;
|
||||||
pub use crate::drone::controller::JoystickInput;
|
|
||||||
use crate::drone::MotorCharacteristics;
|
use crate::drone::JoystickInput;
|
||||||
|
|
||||||
pub struct FPVController {
|
pub struct FPVController {
|
||||||
input: JoystickInput,
|
input: JoystickInput,
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,224 @@
|
||||||
|
#![allow(unused)]
|
||||||
|
use macroquad::prelude as mq;
|
||||||
|
|
||||||
|
#[derive(Default, Clone, Copy, PartialEq)]
|
||||||
|
pub struct JoystickInput {
|
||||||
|
// Value should be between 0 and 1
|
||||||
|
pub throttle_input: f32,
|
||||||
|
|
||||||
|
// Rotation Directions: https://upload.wikimedia.org/wikipedia/commons/c/c1/Yaw_Axis_Corrected.svg
|
||||||
|
/*
|
||||||
|
* Values should be between -1 and 1.
|
||||||
|
*/
|
||||||
|
pub yaw_input: f32,
|
||||||
|
pub pitch_input: f32,
|
||||||
|
pub roll_input: f32,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl JoystickInput {
|
||||||
|
pub fn from_keyboard() -> Self {
|
||||||
|
let mut input = Self::default();
|
||||||
|
if mq::is_key_down(mq::KeyCode::W) {
|
||||||
|
input.throttle_input = 1.0;
|
||||||
|
}
|
||||||
|
if mq::is_key_down(mq::KeyCode::S) {
|
||||||
|
input.throttle_input = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if mq::is_key_down(mq::KeyCode::A) {
|
||||||
|
input.yaw_input = 1.0;
|
||||||
|
} else if mq::is_key_down(mq::KeyCode::D) {
|
||||||
|
input.yaw_input = -1.0;
|
||||||
|
} else {
|
||||||
|
input.yaw_input = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if mq::is_key_down(mq::KeyCode::Up) {
|
||||||
|
input.pitch_input = -1.0;
|
||||||
|
} else if mq::is_key_down(mq::KeyCode::Down) {
|
||||||
|
input.pitch_input = 1.0;
|
||||||
|
} else {
|
||||||
|
input.pitch_input = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
if mq::is_key_down(mq::KeyCode::Left) {
|
||||||
|
input.roll_input = -1.0;
|
||||||
|
} else if mq::is_key_down(mq::KeyCode::Right) {
|
||||||
|
input.roll_input = 1.0;
|
||||||
|
} else {
|
||||||
|
input.roll_input = 0.0;
|
||||||
|
}
|
||||||
|
input = input.clamp();
|
||||||
|
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; // or return an error if you prefer strict parsing
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -1,8 +1,9 @@
|
||||||
|
#![allow(dead_code)]
|
||||||
use nalgebra::{self as na, vector};
|
use nalgebra::{self as na, vector};
|
||||||
use std::{any::Any, f32};
|
use std::{any::Any, f32};
|
||||||
|
|
||||||
use crate::drone::controller::DroneController;
|
use crate::drone::controller::DroneController;
|
||||||
pub use crate::drone::controller::JoystickInput;
|
use crate::drone::JoystickInput;
|
||||||
use crate::drone::MotorCharacteristics;
|
use crate::drone::MotorCharacteristics;
|
||||||
|
|
||||||
const PROPORTIONAL_ONLY: bool = false;
|
const PROPORTIONAL_ONLY: bool = false;
|
||||||
|
|
@ -16,11 +17,6 @@ pub struct PIDControllerState {
|
||||||
angular_velocity: na::Vector3<f32>,
|
angular_velocity: na::Vector3<f32>,
|
||||||
last_error: na::Vector3<f32>,
|
last_error: na::Vector3<f32>,
|
||||||
error_sum: na::Vector3<f32>,
|
error_sum: na::Vector3<f32>,
|
||||||
|
|
||||||
pub log_time: Vec<f32>,
|
|
||||||
pub log_target: Vec<na::Vector3<f32>>,
|
|
||||||
pub log_current: Vec<na::Vector3<f32>>,
|
|
||||||
pub log_error: Vec<na::Vector3<f32>>,
|
|
||||||
}
|
}
|
||||||
|
|
||||||
impl PIDControllerState {
|
impl PIDControllerState {
|
||||||
|
|
@ -30,7 +26,7 @@ impl PIDControllerState {
|
||||||
}
|
}
|
||||||
|
|
||||||
pub struct PIDController {
|
pub struct PIDController {
|
||||||
input: JoystickInput,
|
pub input: JoystickInput,
|
||||||
pub target_rate: f32,
|
pub target_rate: f32,
|
||||||
pub proportional_multiplier: na::Vector3<f32>,
|
pub proportional_multiplier: na::Vector3<f32>,
|
||||||
pub integral_multiplier: na::Vector3<f32>,
|
pub integral_multiplier: na::Vector3<f32>,
|
||||||
|
|
@ -55,30 +51,6 @@ impl Default for PIDController {
|
||||||
|
|
||||||
impl PIDController {
|
impl PIDController {
|
||||||
/// Save log buffers to a CSV file
|
/// Save log buffers to a CSV file
|
||||||
pub fn save_logs_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,target_x,target_y,target_z,current_x,current_y,current_z,error_x,error_y,error_z"
|
|
||||||
)?;
|
|
||||||
|
|
||||||
for i in 0..self.state.log_time.len() {
|
|
||||||
let t = self.state.log_time[i];
|
|
||||||
let tg = self.state.log_target[i];
|
|
||||||
let cur = self.state.log_current[i];
|
|
||||||
let err = self.state.log_error[i];
|
|
||||||
writeln!(
|
|
||||||
file,
|
|
||||||
"{},{},{},{},{},{},{},{},{},{}",
|
|
||||||
t, tg.x, tg.y, tg.z, cur.x, cur.y, cur.z, err.x, err.y, err.z
|
|
||||||
)?;
|
|
||||||
}
|
|
||||||
|
|
||||||
Ok(())
|
|
||||||
}
|
|
||||||
|
|
||||||
pub fn set_input(&mut self, input: JoystickInput) {
|
pub fn set_input(&mut self, input: JoystickInput) {
|
||||||
self.input = input;
|
self.input = input;
|
||||||
|
|
@ -95,6 +67,39 @@ impl PIDController {
|
||||||
) * self.target_rate;
|
) * self.target_rate;
|
||||||
return coords;
|
return coords;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
pub fn get_desired_motor_diffs(&mut self) -> na::Vector3<f32> {
|
||||||
|
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<f32> = target - current;
|
||||||
|
let error_dif: na::Vector3<f32> = error - self.state.last_error;
|
||||||
|
|
||||||
|
let forces_to_apply: na::Vector3<f32> = 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;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
impl DroneController for PIDController {
|
impl DroneController for PIDController {
|
||||||
|
|
@ -118,11 +123,6 @@ impl DroneController for PIDController {
|
||||||
let error: na::Vector3<f32> = target - current;
|
let error: na::Vector3<f32> = target - current;
|
||||||
let throttle = self.input.throttle_input;
|
let throttle = self.input.throttle_input;
|
||||||
|
|
||||||
self.state.log_time.push(self.state.current_time);
|
|
||||||
self.state.log_target.push(target);
|
|
||||||
self.state.log_current.push(current);
|
|
||||||
self.state.log_error.push(error);
|
|
||||||
|
|
||||||
let error_dif: na::Vector3<f32> = error - self.state.last_error;
|
let error_dif: na::Vector3<f32> = error - self.state.last_error;
|
||||||
|
|
||||||
let forces_to_apply = match PROPORTIONAL_ONLY {
|
let forces_to_apply = match PROPORTIONAL_ONLY {
|
||||||
|
|
@ -150,9 +150,9 @@ impl DroneController for PIDController {
|
||||||
self.state.error_sum += error;
|
self.state.error_sum += error;
|
||||||
self.state.last_error = error;
|
self.state.last_error = error;
|
||||||
|
|
||||||
let roll = forces_to_apply.z;
|
|
||||||
let pitch = forces_to_apply.x;
|
let pitch = forces_to_apply.x;
|
||||||
let yaw = forces_to_apply.y;
|
let yaw = forces_to_apply.y;
|
||||||
|
let roll = forces_to_apply.z;
|
||||||
|
|
||||||
let mut motors: [f32; 4] = match self.multiply_mode {
|
let mut motors: [f32; 4] = match self.multiply_mode {
|
||||||
true => {
|
true => {
|
||||||
|
|
@ -169,10 +169,10 @@ impl DroneController for PIDController {
|
||||||
}
|
}
|
||||||
false => {
|
false => {
|
||||||
let t = [
|
let t = [
|
||||||
throttle + yaw - pitch + roll,
|
throttle - pitch + yaw + roll,
|
||||||
throttle - yaw - pitch - roll,
|
throttle - pitch - yaw - roll,
|
||||||
throttle + yaw + pitch - roll,
|
throttle + pitch + yaw - roll,
|
||||||
throttle - yaw + pitch + roll,
|
throttle + pitch - yaw + roll,
|
||||||
];
|
];
|
||||||
t
|
t
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -16,7 +16,7 @@ fn random_color() -> mq::Color {
|
||||||
pub struct World {
|
pub struct World {
|
||||||
physics_pipeline: rp::PhysicsPipeline,
|
physics_pipeline: rp::PhysicsPipeline,
|
||||||
gravity: na::Vector3<f32>,
|
gravity: na::Vector3<f32>,
|
||||||
integration_parameters: rp::IntegrationParameters,
|
pub integration_parameters: rp::IntegrationParameters,
|
||||||
island_manager: rp::IslandManager,
|
island_manager: rp::IslandManager,
|
||||||
broad_phase: rp::BroadPhaseBvh,
|
broad_phase: rp::BroadPhaseBvh,
|
||||||
narrow_phase: rp::NarrowPhase,
|
narrow_phase: rp::NarrowPhase,
|
||||||
|
|
@ -37,6 +37,38 @@ pub struct ColliderExtraData {
|
||||||
}
|
}
|
||||||
|
|
||||||
impl World {
|
impl World {
|
||||||
|
pub fn new(dt: f32) -> Self {
|
||||||
|
let gravity: na::Vector3<f32> = rp::vector![0.0, -9.81, 0.0];
|
||||||
|
let mut integration_parameters = rp::IntegrationParameters::default();
|
||||||
|
integration_parameters.set_inv_dt(dt);
|
||||||
|
let physics_pipeline = rp::PhysicsPipeline::new();
|
||||||
|
let island_manager = rp::IslandManager::new();
|
||||||
|
let broad_phase = rp::DefaultBroadPhase::new();
|
||||||
|
let narrow_phase = rp::NarrowPhase::new();
|
||||||
|
let impulse_joint_set = rp::ImpulseJointSet::new();
|
||||||
|
let multibody_joint_set = rp::MultibodyJointSet::new();
|
||||||
|
let ccd_solver = rp::CCDSolver::new();
|
||||||
|
let physics_hooks = ();
|
||||||
|
let event_handler = ();
|
||||||
|
return Self {
|
||||||
|
physics_pipeline,
|
||||||
|
gravity: gravity,
|
||||||
|
integration_parameters,
|
||||||
|
island_manager,
|
||||||
|
broad_phase,
|
||||||
|
narrow_phase,
|
||||||
|
impulse_joint_set,
|
||||||
|
multibody_joint_set,
|
||||||
|
ccd_solver,
|
||||||
|
hooks: Box::new(physics_hooks),
|
||||||
|
events: Box::new(event_handler),
|
||||||
|
bodies: rp::RigidBodySet::new(),
|
||||||
|
colliders: rp::ColliderSet::new(),
|
||||||
|
collider_data: HashMap::new(),
|
||||||
|
tick: 0,
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
pub fn step(&mut self) {
|
pub fn step(&mut self) {
|
||||||
self.physics_pipeline.step(
|
self.physics_pipeline.step(
|
||||||
&self.gravity,
|
&self.gravity,
|
||||||
|
|
@ -59,6 +91,7 @@ impl World {
|
||||||
return self.integration_parameters.dt * (self.tick as f32);
|
return self.integration_parameters.dt * (self.tick as f32);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#[allow(dead_code)]
|
||||||
pub fn clear_ofb(&mut self) {
|
pub fn clear_ofb(&mut self) {
|
||||||
let mut coll_to_del: Vec<ColliderHandle> = Vec::new();
|
let mut coll_to_del: Vec<ColliderHandle> = Vec::new();
|
||||||
for (handle, col) in self.colliders.iter() {
|
for (handle, col) in self.colliders.iter() {
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,11 @@
|
||||||
|
use std::fs;
|
||||||
|
use std::path::{Path, PathBuf};
|
||||||
|
|
||||||
|
pub fn list_files(dir: &str) -> Vec<PathBuf> {
|
||||||
|
fs::read_dir(dir)
|
||||||
|
.expect("Failed to read directory")
|
||||||
|
.filter_map(|e| e.ok())
|
||||||
|
.map(|e| e.path())
|
||||||
|
.filter(|p| p.is_file())
|
||||||
|
.collect()
|
||||||
|
}
|
||||||
267
src/main.rs
267
src/main.rs
|
|
@ -1,55 +1,140 @@
|
||||||
use gilrs;
|
|
||||||
use macroquad::prelude as mq;
|
use macroquad::prelude as mq;
|
||||||
use nalgebra as na;
|
|
||||||
use rapier3d::prelude::*;
|
|
||||||
|
|
||||||
mod engine;
|
mod engine;
|
||||||
use engine::*;
|
use engine::*;
|
||||||
|
|
||||||
mod camera;
|
mod camera;
|
||||||
|
mod config;
|
||||||
mod drone;
|
mod drone;
|
||||||
|
mod helpers;
|
||||||
mod rendering;
|
mod rendering;
|
||||||
|
mod simulation;
|
||||||
|
|
||||||
mod graphics_util;
|
mod graphics_util;
|
||||||
|
|
||||||
use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};
|
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/";
|
||||||
|
const RESULTS_DIR: &str = "results/";
|
||||||
|
|
||||||
fn window_conf() -> mq::Conf {
|
fn window_conf() -> mq::Conf {
|
||||||
mq::Conf {
|
mq::Conf {
|
||||||
window_title: "RustDroneSim".to_owned(),
|
window_title: "RustDroneSim".to_owned(),
|
||||||
window_resizable: true,
|
window_resizable: true,
|
||||||
// fullscreen: true,
|
|
||||||
platform: mq::miniquad::conf::Platform {
|
platform: mq::miniquad::conf::Platform {
|
||||||
// linux_backend: mq::miniquad::conf::LinuxBackend::WaylandOnly,
|
|
||||||
..Default::default()
|
..Default::default()
|
||||||
},
|
},
|
||||||
..Default::default()
|
..Default::default()
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
use std::env;
|
||||||
|
|
||||||
|
enum RunMode {
|
||||||
|
Batch,
|
||||||
|
Record { output: String },
|
||||||
|
}
|
||||||
|
|
||||||
|
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)]
|
#[macroquad::main(window_conf)]
|
||||||
async fn main() {
|
async fn main() {
|
||||||
/* World Setup */
|
match parse_cli() {
|
||||||
let mut world = World::default();
|
RunMode::Batch => run_batch().await,
|
||||||
|
RunMode::Record { output } => run_record(output).await,
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
world.register_free_collider(
|
async fn run_batch() {
|
||||||
ColliderBuilder::cuboid(30.0, 1.0, 30.0)
|
fs::create_dir_all(RESULTS_DIR).unwrap();
|
||||||
.translation(vector![0.0, -2.0, 0.0])
|
|
||||||
.restitution(0.5)
|
let input_files = list_files(INPUTS_DIR);
|
||||||
.build(),
|
let config_files = list_files(CONFIGS_DIR);
|
||||||
None,
|
|
||||||
|
for input_path in input_files {
|
||||||
|
let input_name = input_path.file_stem().unwrap().to_string_lossy();
|
||||||
|
|
||||||
|
let inputs = InputRecording::load_inputs_from_csv(input_path.to_str().unwrap())
|
||||||
|
.expect("Failed to load input recording");
|
||||||
|
|
||||||
|
for config_path in &config_files {
|
||||||
|
let config_name = config_path.file_stem().unwrap().to_string_lossy();
|
||||||
|
|
||||||
|
let config: SimulationConfig =
|
||||||
|
toml::from_str(&fs::read_to_string(config_path).unwrap())
|
||||||
|
.expect("Invalid config file");
|
||||||
|
|
||||||
|
println!(
|
||||||
|
"Running simulation: input={} config={}",
|
||||||
|
input_name, config_name
|
||||||
);
|
);
|
||||||
|
|
||||||
let mut drone_controller = drone::pidcontroller::PIDController::default();
|
/* World */
|
||||||
drone_controller.set_input(JoystickInput {
|
let mut world = World::new(config.tickrate);
|
||||||
throttle_input: 0.75,
|
|
||||||
yaw_input: 0.4,
|
/* Drone */
|
||||||
roll_input: 0.7,
|
let drone = drone::Drone::new(
|
||||||
pitch_input: 0.0,
|
|
||||||
});
|
|
||||||
let mut drone_obj = drone::Drone::new(
|
|
||||||
&mut world,
|
&mut world,
|
||||||
Box::new(drone_controller),
|
Box::new(drone::pidcontroller::PIDController {
|
||||||
|
target_rate: config.target_rate,
|
||||||
|
proportional_multiplier: config.proportional(),
|
||||||
|
integral_multiplier: config.integral(),
|
||||||
|
diferential_multiplier: config.diferential(),
|
||||||
|
..Default::default()
|
||||||
|
}),
|
||||||
|
drone::MotorCharacteristics {
|
||||||
|
max_thrust: config.max_thrust,
|
||||||
|
max_torque: config.max_torque,
|
||||||
|
..Default::default()
|
||||||
|
},
|
||||||
|
config.mass,
|
||||||
|
);
|
||||||
|
|
||||||
|
let result_file = format!("{}/{}_{}.csv", RESULTS_DIR, input_name, config_name);
|
||||||
|
|
||||||
|
let mut sim = Simulation::new(
|
||||||
|
drone,
|
||||||
|
world,
|
||||||
|
false,
|
||||||
|
SimMode::Playback(inputs.clone(), 0.0),
|
||||||
|
Some(result_file),
|
||||||
|
config.drone_tick_rate,
|
||||||
|
);
|
||||||
|
|
||||||
|
sim.run().await.unwrap();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
println!("All simulations completed.");
|
||||||
|
}
|
||||||
|
|
||||||
|
async fn run_record(output: String) {
|
||||||
|
println!("Recording inputs to {}", output);
|
||||||
|
|
||||||
|
let tickrate = 60000.0;
|
||||||
|
let mut world = World::new(tickrate);
|
||||||
|
|
||||||
|
let drone = drone::Drone::new(
|
||||||
|
&mut world,
|
||||||
|
Box::new(drone::pidcontroller::PIDController {
|
||||||
|
..Default::default()
|
||||||
|
}),
|
||||||
drone::MotorCharacteristics {
|
drone::MotorCharacteristics {
|
||||||
max_thrust: 2.6,
|
max_thrust: 2.6,
|
||||||
max_torque: 0.5,
|
max_torque: 0.5,
|
||||||
|
|
@ -58,132 +143,14 @@ async fn main() {
|
||||||
0.350,
|
0.350,
|
||||||
);
|
);
|
||||||
|
|
||||||
/* Renderer Setup */
|
let mut sim = Simulation::new(
|
||||||
let camera = camera::AttachedCamera::new(
|
drone,
|
||||||
drone_obj.rb_handle,
|
world,
|
||||||
vector![1.0, 0.0, 0.0],
|
true,
|
||||||
vector![0.5, 0.0, 0.0],
|
SimMode::Record(InputRecording::default(), output),
|
||||||
);
|
|
||||||
let mut renderer = Renderer::new(Box::new(camera));
|
|
||||||
|
|
||||||
renderer.light.set_location(
|
|
||||||
vector![70.0, 150.0, -90.0].into(),
|
|
||||||
vector![-0.4, -0.7, 0.6].into(),
|
|
||||||
);
|
|
||||||
renderer.update_light(&world);
|
|
||||||
|
|
||||||
/* Command Setup */
|
|
||||||
// let mut gilrs = gilrs::Gilrs::new().unwrap();
|
|
||||||
//
|
|
||||||
// // Iterate over all connected gamepads
|
|
||||||
// for (_id, gamepad) in gilrs.gamepads() {
|
|
||||||
// println!("{} is {:?}", gamepad.name(), gamepad.power_info());
|
|
||||||
// }
|
|
||||||
|
|
||||||
let mut input = JoystickInput::default();
|
|
||||||
|
|
||||||
loop {
|
|
||||||
renderer.update_camera(&world);
|
|
||||||
if mq::is_key_pressed(mq::KeyCode::L) {
|
|
||||||
renderer
|
|
||||||
.light
|
|
||||||
.set_location(renderer.camera.get_position(), renderer.camera.get_front());
|
|
||||||
renderer.update_light(&world);
|
|
||||||
}
|
|
||||||
|
|
||||||
if mq::is_key_pressed(mq::KeyCode::Q) {
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
if mq::is_key_pressed(mq::KeyCode::C) {
|
|
||||||
add_objects(&mut world);
|
|
||||||
}
|
|
||||||
if mq::is_key_pressed(mq::KeyCode::M) {
|
|
||||||
renderer.apply_config();
|
|
||||||
}
|
|
||||||
|
|
||||||
if mq::is_key_down(mq::KeyCode::W) {
|
|
||||||
input.throttle_input += 4.0 * mq::get_frame_time();
|
|
||||||
}
|
|
||||||
if mq::is_key_down(mq::KeyCode::S) {
|
|
||||||
input.throttle_input -= 4.0 * mq::get_frame_time();
|
|
||||||
}
|
|
||||||
|
|
||||||
if mq::is_key_down(mq::KeyCode::A) {
|
|
||||||
input.yaw_input = 1.0;
|
|
||||||
} else if mq::is_key_down(mq::KeyCode::D) {
|
|
||||||
input.yaw_input = -1.0;
|
|
||||||
} else {
|
|
||||||
input.yaw_input = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if mq::is_key_down(mq::KeyCode::Up) {
|
|
||||||
input.pitch_input = -1.0;
|
|
||||||
} else if mq::is_key_down(mq::KeyCode::Down) {
|
|
||||||
input.pitch_input = 1.0;
|
|
||||||
} else {
|
|
||||||
input.pitch_input = 0.0;
|
|
||||||
}
|
|
||||||
|
|
||||||
if mq::is_key_down(mq::KeyCode::Left) {
|
|
||||||
input.roll_input = -1.0;
|
|
||||||
} else if mq::is_key_down(mq::KeyCode::Right) {
|
|
||||||
input.roll_input = 1.0;
|
|
||||||
} else {
|
|
||||||
input.roll_input = 0.0;
|
|
||||||
}
|
|
||||||
input = input.clamp();
|
|
||||||
match drone_obj
|
|
||||||
.controller
|
|
||||||
.as_mut_any()
|
|
||||||
.downcast_mut::<drone::pidcontroller::PIDController>()
|
|
||||||
{
|
|
||||||
Some(cont) => {
|
|
||||||
cont.set_input(input);
|
|
||||||
}
|
|
||||||
None => {}
|
|
||||||
};
|
|
||||||
|
|
||||||
// Physics
|
|
||||||
world.step();
|
|
||||||
let _ = clearscreen::clear();
|
|
||||||
drone_obj.process_tick(&mut world);
|
|
||||||
|
|
||||||
// Rendering
|
|
||||||
renderer.draw(&mut world);
|
|
||||||
|
|
||||||
if world.tick % (20) == 0 {
|
|
||||||
// renderer.update_light(&world);
|
|
||||||
// world.clear_ofb();
|
|
||||||
mq::next_frame().await;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
match drone_obj
|
|
||||||
.controller
|
|
||||||
.as_mut_any()
|
|
||||||
.downcast_mut::<drone::pidcontroller::PIDController>()
|
|
||||||
{
|
|
||||||
Some(cont) => {
|
|
||||||
let _ = cont.save_logs_to_csv("./pid_logs.csv");
|
|
||||||
}
|
|
||||||
None => {}
|
|
||||||
};
|
|
||||||
}
|
|
||||||
|
|
||||||
fn add_objects(world: &mut World) {
|
|
||||||
for i in 0..1 {
|
|
||||||
let body = world.register_body(
|
|
||||||
RigidBodyBuilder::dynamic()
|
|
||||||
.translation(vector![0.0, 50.0 + i as f32, 0.0])
|
|
||||||
.rotation(vector![std::f32::consts::PI / 4.2, i as f32, i as f32])
|
|
||||||
.build(),
|
|
||||||
);
|
|
||||||
world.register_collider(
|
|
||||||
ColliderBuilder::cuboid(2.0, 2.0, 2.0)
|
|
||||||
.restitution(0.1)
|
|
||||||
.build(),
|
|
||||||
body,
|
|
||||||
None,
|
None,
|
||||||
|
600,
|
||||||
);
|
);
|
||||||
}
|
|
||||||
|
sim.run().await.unwrap();
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,253 @@
|
||||||
|
use macroquad::prelude as mq;
|
||||||
|
use nalgebra as na;
|
||||||
|
use rapier3d::prelude as rp;
|
||||||
|
use std::error::Error;
|
||||||
|
|
||||||
|
use crate::{
|
||||||
|
drone::{
|
||||||
|
controller::DroneController,
|
||||||
|
input::{InputRecording, JoystickInput},
|
||||||
|
Drone,
|
||||||
|
},
|
||||||
|
engine::World,
|
||||||
|
rendering::Renderer,
|
||||||
|
};
|
||||||
|
|
||||||
|
pub enum SimMode {
|
||||||
|
Record(InputRecording, String),
|
||||||
|
Playback(InputRecording, f32),
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct DataResultRecord {
|
||||||
|
pub time: f32,
|
||||||
|
pub current_angular_velocity: na::Vector3<f32>,
|
||||||
|
pub target_angular_velocity: na::Vector3<f32>,
|
||||||
|
pub applied_motor_offset: na::Vector3<f32>,
|
||||||
|
pub desired_motor_offset: na::Vector3<f32>,
|
||||||
|
}
|
||||||
|
|
||||||
|
#[derive(Default)]
|
||||||
|
pub struct SimulationState {
|
||||||
|
pub data_results: Vec<DataResultRecord>,
|
||||||
|
}
|
||||||
|
|
||||||
|
pub struct Simulation {
|
||||||
|
pub drone: Drone,
|
||||||
|
pub world: World,
|
||||||
|
pub renderer: Option<Renderer>,
|
||||||
|
pub mode: SimMode,
|
||||||
|
results_file: Option<String>,
|
||||||
|
drone_tick_rate: u64,
|
||||||
|
state: SimulationState,
|
||||||
|
}
|
||||||
|
|
||||||
|
impl Simulation {
|
||||||
|
pub fn new(
|
||||||
|
drone: Drone,
|
||||||
|
world: World,
|
||||||
|
render: bool,
|
||||||
|
mode: SimMode,
|
||||||
|
results_file: Option<String>,
|
||||||
|
drone_tick_rate: u64,
|
||||||
|
) -> Self {
|
||||||
|
let renderer: Option<Renderer> = match render {
|
||||||
|
true => Some(Renderer::new(Box::new(crate::camera::AttachedCamera::new(
|
||||||
|
drone.rb_handle,
|
||||||
|
na::vector![1.0, 0.0, 0.0],
|
||||||
|
na::vector![0.5, 0.0, 0.0],
|
||||||
|
)))),
|
||||||
|
false => None,
|
||||||
|
};
|
||||||
|
|
||||||
|
let mut s = Self {
|
||||||
|
drone,
|
||||||
|
world,
|
||||||
|
renderer,
|
||||||
|
mode,
|
||||||
|
results_file,
|
||||||
|
drone_tick_rate,
|
||||||
|
state: SimulationState::default(),
|
||||||
|
};
|
||||||
|
|
||||||
|
s.world.register_free_collider(
|
||||||
|
rp::ColliderBuilder::cuboid(30.0, 1.0, 30.0)
|
||||||
|
.translation(na::vector![0.0, -2.0, 0.0])
|
||||||
|
.restitution(0.5)
|
||||||
|
.sensor(true)
|
||||||
|
.build(),
|
||||||
|
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 async fn run(&mut self) -> Result<(), Box<dyn Error>> {
|
||||||
|
let mut current_input: JoystickInput;
|
||||||
|
|
||||||
|
loop {
|
||||||
|
if let Some(renderer) = &mut self.renderer {
|
||||||
|
renderer.update_camera(&self.world);
|
||||||
|
}
|
||||||
|
|
||||||
|
if mq::is_key_pressed(mq::KeyCode::Q) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- Input handling ---
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
SimMode::Playback(recording, start_time) => {
|
||||||
|
let playback_time = current_time - *start_time;
|
||||||
|
current_input = recording.get_input(playback_time);
|
||||||
|
|
||||||
|
if recording.ended(playback_time) {
|
||||||
|
println!("Playback ended.");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// --- Physics ---
|
||||||
|
self.world.step();
|
||||||
|
if self.world.tick
|
||||||
|
% ((self.world.integration_parameters.inv_dt() / self.drone_tick_rate as f32)
|
||||||
|
as u64)
|
||||||
|
== 0
|
||||||
|
{
|
||||||
|
if let Some(cont) = self
|
||||||
|
.drone
|
||||||
|
.controller
|
||||||
|
.as_mut_any()
|
||||||
|
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||||
|
{
|
||||||
|
cont.set_input(current_input);
|
||||||
|
}
|
||||||
|
self.drone.process_tick(&mut self.world);
|
||||||
|
}
|
||||||
|
|
||||||
|
let target_angular_vel: na::Vector3<f32>;
|
||||||
|
let desired_motor_diff: na::Vector3<f32>;
|
||||||
|
let applied_motor_diff: na::Vector3<f32>;
|
||||||
|
if let Some(cont) = self
|
||||||
|
.drone
|
||||||
|
.controller
|
||||||
|
.as_mut_any()
|
||||||
|
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
|
||||||
|
{
|
||||||
|
desired_motor_diff = cont.get_desired_motor_diffs();
|
||||||
|
target_angular_vel = cont.get_desired_angular_velocity();
|
||||||
|
|
||||||
|
/*
|
||||||
|
let t = [
|
||||||
|
throttle - pitch + yaw + roll,
|
||||||
|
throttle - pitch - yaw - roll,
|
||||||
|
throttle + pitch + yaw - roll,
|
||||||
|
throttle + pitch - yaw + roll,
|
||||||
|
];
|
||||||
|
*/
|
||||||
|
let m = cont.get_motor_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])
|
||||||
|
] / 4.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,
|
||||||
|
});
|
||||||
|
|
||||||
|
// --- 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().await;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
self.shutdown()?;
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
pub fn save_logs_to_csv(&self, path: &str) -> std::io::Result<()> {
|
||||||
|
use std::fs::File;
|
||||||
|
use std::io::{BufWriter, Write};
|
||||||
|
let file = File::create(path)?;
|
||||||
|
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"
|
||||||
|
)?;
|
||||||
|
|
||||||
|
for entry in &self.state.data_results {
|
||||||
|
let tg = entry.target_angular_velocity;
|
||||||
|
let cur = entry.current_angular_velocity;
|
||||||
|
let err = tg - cur;
|
||||||
|
let mot = entry.applied_motor_offset;
|
||||||
|
let dmot = entry.desired_motor_offset;
|
||||||
|
|
||||||
|
writeln!(
|
||||||
|
writer,
|
||||||
|
"{},{},{},{},{},{},{},{},{},{},{},{},{},{},{},{}",
|
||||||
|
entry.time,
|
||||||
|
tg.x,
|
||||||
|
tg.y,
|
||||||
|
tg.z,
|
||||||
|
cur.x,
|
||||||
|
cur.y,
|
||||||
|
cur.z,
|
||||||
|
err.x,
|
||||||
|
err.y,
|
||||||
|
err.z,
|
||||||
|
mot.x,
|
||||||
|
mot.y,
|
||||||
|
mot.z,
|
||||||
|
dmot.x,
|
||||||
|
dmot.y,
|
||||||
|
dmot.z
|
||||||
|
)?;
|
||||||
|
}
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
|
||||||
|
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)?;
|
||||||
|
println!("Input recording saved to {}", dest);
|
||||||
|
}
|
||||||
|
if let Some(filename) = &self.results_file {
|
||||||
|
self.save_logs_to_csv(&filename)?;
|
||||||
|
}
|
||||||
|
|
||||||
|
Ok(())
|
||||||
|
}
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue