diff --git a/configurations/gen.py b/configurations/gen.py index 8ed7816..1e7953e 100644 --- a/configurations/gen.py +++ b/configurations/gen.py @@ -1,11 +1,14 @@ import os import subprocess -subprocess.run("rm -rf final && mkdir final", shell=True) +FOLDER_PREFIX = "configurations/" +MOT_FOLDER = FOLDER_PREFIX + "mot/" +SIM_FOLDER = FOLDER_PREFIX + "sim/" +PID_FOLDER = FOLDER_PREFIX + "pid_cont/" +FINAL_FOLDER = FOLDER_PREFIX + "final/" + +subprocess.run(f"rm -rf {FINAL_FOLDER} && mkdir {FINAL_FOLDER}", shell=True) -MOT_FOLDER = "mot/" -SIM_FOLDER = "sim/" -PID_FOLDER = "pid_cont/" mot_files = os.listdir(MOT_FOLDER) sim_files = os.listdir(SIM_FOLDER) @@ -22,7 +25,7 @@ for case in prod: with open(PID_FOLDER + case[2], "r") as f3: settings = f1.read() + "\n" + f2.read() + "\n" + f3.read() with open( - "final/" + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml", + FINAL_FOLDER + "".join(case).replace(".toml", "_").removesuffix("_") + ".toml", "w", ) as f: f.write(settings) diff --git a/configurations/mot/mot_std.toml b/configurations/mot/mot_std.toml index ec4e19f..a7f5a41 100644 --- a/configurations/mot/mot_std.toml +++ b/configurations/mot/mot_std.toml @@ -5,9 +5,10 @@ mass = 0.350 time_constant = 0.00 +# roll, pitch, yaw motor_map = [ - [-1.0, 1.0, 1.0], # Front Right - [1.0, -1.0, 1.0], # Front Left - [1.0, 1.0, -1.0], # Rear Left - [-1.0, -1.0, -1.0], # Rear Right + [1.0, -1.0, 1.0], # Front Right + [-1.0, -1.0, -1.0], # Front Left + [-1.0, 1.0, 1.0], # Rear Left + [1.0, 1.0, -1.0], # Rear Right ] diff --git a/configurations/mot/mot_tc.toml b/configurations/mot/mot_tc.toml deleted file mode 100644 index 7081bc6..0000000 --- a/configurations/mot/mot_tc.toml +++ /dev/null @@ -1,12 +0,0 @@ -max_thrust = 2.6 -max_torque = 0.5 -mass = 0.350 - -time_constant = 0.01 - -motor_map = [ - [-1.0, 1.0, 1.0], # Front Right - [1.0, -1.0, 1.0], # Front Left - [1.0, 1.0, -1.0], # Rear Left - [-1.0, -1.0, -1.0], # Rear Right -] diff --git a/configurations/pid_cont/rate_025.toml b/configurations/pid_cont/rate_025.toml index dfad1eb..fedb3db 100644 --- a/configurations/pid_cont/rate_025.toml +++ b/configurations/pid_cont/rate_025.toml @@ -12,9 +12,9 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [ 0.0, 0.0, ], frequency = 50.0 }, rate_pid = { kp = [ - 0.03, - 0.3, - 0.03, + 0.1, + 0.1, + 1.0, ], ki = [ 0.0, 0.0, diff --git a/configurations/pid_cont/rate_030.toml b/configurations/pid_cont/rate_030.toml new file mode 100644 index 0000000..61ece75 --- /dev/null +++ b/configurations/pid_cont/rate_030.toml @@ -0,0 +1,26 @@ + +stack = { max_rate = 3.14, rotation_pid = { kp = [ + 1.5, + 1.5, + 1.5, +], ki = [ + 0.0, + 0.0, + 0.0, +], kd = [ + 0.0, + 0.0, + 0.0, +], frequency = 50.0 }, rate_pid = { kp = [ + 0.2, + 0.2, + 2.0, +], ki = [ + 0.0, + 0.0, + 0.0, +], kd = [ + 0.0, + 0.0, + 0.0, +], frequency = 600.0 } } diff --git a/configurations/pid_cont/rate_03.toml b/configurations/pid_cont_old/rate_03_x.toml similarity index 94% rename from configurations/pid_cont/rate_03.toml rename to configurations/pid_cont_old/rate_03_x.toml index fbec278..f9ebf2c 100644 --- a/configurations/pid_cont/rate_03.toml +++ b/configurations/pid_cont_old/rate_03_x.toml @@ -13,8 +13,8 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [ 0.0, ], frequency = 50.0 }, rate_pid = { kp = [ 0.03, - 0.3, - 0.03, + 0.0, + 0.0, ], ki = [ 0.0, 0.0, diff --git a/configurations/pid_cont/rate_02.toml b/configurations/pid_cont_old/rate_03_y.toml similarity index 72% rename from configurations/pid_cont/rate_02.toml rename to configurations/pid_cont_old/rate_03_y.toml index d443470..129ec43 100644 --- a/configurations/pid_cont/rate_02.toml +++ b/configurations/pid_cont_old/rate_03_y.toml @@ -1,8 +1,8 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [ - 0.5, - 0.5, - 0.5, + 1.0, + 1.0, + 1.0, ], ki = [ 0.0, 0.0, @@ -11,10 +11,10 @@ stack = { max_rate = 3.14, rotation_pid = { kp = [ 0.0, 0.0, 0.0, -], frequency = 100.0 }, rate_pid = { kp = [ - 0.03, - 0.3, +], frequency = 50.0 }, rate_pid = { kp = [ + 0.0, 0.03, + 0.0, ], ki = [ 0.0, 0.0, diff --git a/configurations/pid_cont_old/rate_03_z.toml b/configurations/pid_cont_old/rate_03_z.toml new file mode 100644 index 0000000..85b771f --- /dev/null +++ b/configurations/pid_cont_old/rate_03_z.toml @@ -0,0 +1,26 @@ + +stack = { max_rate = 3.14, rotation_pid = { kp = [ + 1.0, + 1.0, + 1.0, +], ki = [ + 0.0, + 0.0, + 0.0, +], kd = [ + 0.0, + 0.0, + 0.0, +], frequency = 50.0 }, rate_pid = { kp = [ + 0.0, + 0.0, + 0.3, +], ki = [ + 0.0, + 0.0, + 0.0, +], kd = [ + 0.0, + 0.0, + 0.0, +], frequency = 600.0 } } diff --git a/inputs/not/rot_all.json b/inputs/not/rot_all.json new file mode 100644 index 0000000..c8cbc60 --- /dev/null +++ b/inputs/not/rot_all.json @@ -0,0 +1,92 @@ +{ + "records": [ + { + "input": { + "joystick": { + "throttle_input": 0, + "roll_input": 0, + "yaw_input": 0, + "pitch_input": 0 + }, + "position": { + "lat": 0, + "long": 0, + "alt": 0 + }, + "rotation": { + "roll": 0, + "yaw": 0, + "pitch": 0 + }, + "mode": "Rotation" + }, + "time": 0 + }, + { + "input": { + "joystick": { + "throttle_input": 0, + "roll_input": 0, + "yaw_input": 0, + "pitch_input": 0 + }, + "position": { + "lat": 0, + "long": 0, + "alt": 0 + }, + "rotation": { + "roll": 3.14, + "yaw": 3.1, + "pitch": 1.5 + }, + "mode": "Rotation" + }, + "time": 3 + }, + { + "input": { + "joystick": { + "throttle_input": 0, + "roll_input": 0, + "yaw_input": 0, + "pitch_input": 0 + }, + "position": { + "lat": 0, + "long": 0, + "alt": 0 + }, + "rotation": { + "roll": -1.5, + "yaw": 0.8, + "pitch": 0.4 + }, + "mode": "Rotation" + }, + "time": 10 + }, + { + "input": { + "joystick": { + "throttle_input": 0, + "roll_input": 0, + "yaw_input": 0, + "pitch_input": 0 + }, + "position": { + "lat": 0, + "long": 0, + "alt": 0 + }, + "rotation": { + "roll": 1.5, + "yaw": 0.1, + "pitch": 2 + }, + "mode": "Rotation" + }, + "time": 20 + } + ] +} diff --git a/inputs/rot_x.json b/inputs/not/rot_x.json similarity index 100% rename from inputs/rot_x.json rename to inputs/not/rot_x.json diff --git a/inputs/rot_y.json b/inputs/not/rot_y.json similarity index 100% rename from inputs/rot_y.json rename to inputs/not/rot_y.json diff --git a/inputs/rot_z.json b/inputs/not/rot_z.json similarity index 100% rename from inputs/rot_z.json rename to inputs/not/rot_z.json diff --git a/src/config.rs b/src/config.rs index a66cb75..d5f88d9 100644 --- a/src/config.rs +++ b/src/config.rs @@ -2,7 +2,7 @@ use nalgebra::{vector, Vector3}; use rapier3d::prelude::*; use serde::Deserialize; -#[derive(Debug, Deserialize, Clone)] +#[derive(Default, Debug, Deserialize, Clone)] pub struct PidConfig { pub kp: [f32; 3], pub ki: [f32; 3], @@ -26,8 +26,10 @@ impl PidConfig { #[derive(Debug, Deserialize, Clone)] pub struct ControllerStackConfig { /// PID for the rotation (angle → angular rate) layer + #[serde(default)] pub rotation_pid: PidConfig, + #[serde(default)] pub acceleration_pid: PidConfig, /// PID for the angular rate (angular rate → torque) layer diff --git a/src/drone.rs b/src/drone.rs index 9111494..c3d9bbb 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -34,10 +34,10 @@ impl Default for MotorCharacteristics { * 2 --- 3 */ relative_motor_positions: [ - rp::point![5.0, 0.0, 5.0], - rp::point![5.0, 0.0, -5.0], - rp::point![-5.0, 0.0, -5.0], - rp::point![-5.0, 0.0, 5.0], + rp::point![5.0, 5.0, 0.0], + rp::point![5.0, -5.0, 0.0], + rp::point![-5.0, -5.0, 0.0], + rp::point![-5.0, 5.0, 0.0], ], max_thrust: 2.6, max_torque: 0.5, @@ -70,7 +70,7 @@ impl Drone { ) -> Drone { let drone_rb_handle = world.register_body( rp::RigidBodyBuilder::dynamic() - .translation(rp::vector![0.0, 10.0, 0.0]) + .translation(rp::vector![0.0, 0.0, 10.0]) .rotation(rp::vector![0.0, 0.0, 0.0]) /* * These damping values keep the simulation more realistic, @@ -164,7 +164,7 @@ impl Drone { // Apply RB's rotation to point at RB's Up // // motor_position is relative, transform it by the RB's position first - let mut thrust_force = nalgebra::Vector3::new(0.0, thrust, 0.0); + let mut thrust_force = nalgebra::Vector3::new(0.0, 0.0, thrust); thrust_force = drone_rb.rotation().transform_vector(&thrust_force); drone_rb.add_force_at_point( thrust_force, @@ -173,9 +173,9 @@ impl Drone { ); let torque = if i % 2 == 0 { - rp::vector![0.0, torque, 0.0] + rp::vector![0.0, 0.0, torque] } else { - rp::vector![0.0, -torque, 0.0] + rp::vector![0.0, 0.0, -torque] }; drone_rb.add_torque(drone_rb.rotation().transform_vector(&torque), true); } diff --git a/src/drone/input.rs b/src/drone/input.rs index b0ed180..ba67f5b 100644 --- a/src/drone/input.rs +++ b/src/drone/input.rs @@ -44,9 +44,13 @@ pub enum ModeInput { #[derive(Default, Clone, Copy, PartialEq, serde::Serialize, serde::Deserialize)] pub struct Input { + #[serde(default)] pub joystick: JoystickInput, + #[serde(default)] pub acceleration: AccelerationInput, + #[serde(default)] pub rotation: RotationInput, + #[serde(default)] pub position: PositionInput, pub mode: ModeInput, } diff --git a/src/drone/stacked.rs b/src/drone/stacked.rs index b07aada..080f3ca 100644 --- a/src/drone/stacked.rs +++ b/src/drone/stacked.rs @@ -86,8 +86,8 @@ impl DroneController for StackedController { // println!("Rotation!"); Rotation(na::vector![ self.input.rotation.roll, - self.input.rotation.yaw, self.input.rotation.pitch, + self.input.rotation.yaw, ]) }; self.rotation_rt @@ -96,8 +96,8 @@ impl DroneController for StackedController { AngularRate( na::vector![ self.input.joystick.roll_input, - self.input.joystick.yaw_input, self.input.joystick.pitch_input, + self.input.joystick.yaw_input, ] * self.config.stack.max_rate, ) }; diff --git a/src/drone/stacked/mixer.rs b/src/drone/stacked/mixer.rs index dd124fb..809e43f 100644 --- a/src/drone/stacked/mixer.rs +++ b/src/drone/stacked/mixer.rs @@ -12,7 +12,7 @@ impl Default for MotorMixingMode { } pub struct MotorMixer { - pub motor_map: [[f32; 3]; 4], // roll, yaw, pitch + pub motor_map: [[f32; 3]; 4], // roll, pitch, yaw pub min_throttle: f32, pub max_throttle: f32, pub mixing_mode: MotorMixingMode, @@ -77,12 +77,14 @@ impl MotorMixer { scale = scale.clamp(min_scale, 1.0); - let max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0); - let min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0); + let mut max_delta = scale * delta.into_iter().reduce(f32::max).unwrap_or(0.0); + let mut min_delta = scale * delta.into_iter().reduce(f32::min).unwrap_or(0.0); let delta_dif = max_delta - min_delta; if delta_dif > throttle_range_len { - scale *= throttle_range_len / delta_dif; + scale *= throttle_range_len / delta_dif * 0.9; + max_delta *= throttle_range_len / delta_dif * 0.9; + min_delta *= throttle_range_len / delta_dif * 0.9; } let lim_throttle = throttle.clamp( diff --git a/src/engine.rs b/src/engine.rs index 4e5684d..3cb514c 100644 --- a/src/engine.rs +++ b/src/engine.rs @@ -38,7 +38,7 @@ pub struct ColliderExtraData { impl World { pub fn new(dt: f32) -> Self { - let gravity: na::Vector3 = rp::vector![0.0, -9.81, 0.0]; + let gravity: na::Vector3 = rp::vector![0.0, 0.0, -9.81]; let mut integration_parameters = rp::IntegrationParameters::default(); integration_parameters.set_inv_dt(dt); let physics_pipeline = rp::PhysicsPipeline::new(); @@ -153,7 +153,7 @@ impl World { impl Default for World { fn default() -> Self { - let gravity: na::Vector3 = rp::vector![0.0, -9.81, 0.0]; + let gravity: na::Vector3 = rp::vector![0.0, 0.0, -9.81]; let mut integration_parameters = rp::IntegrationParameters::default(); integration_parameters.set_inv_dt(600.0); let physics_pipeline = rp::PhysicsPipeline::new(); diff --git a/src/simulation.rs b/src/simulation.rs index 731d7ec..4ff923b 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -6,7 +6,7 @@ use std::error::Error; use crate::{ drone::{ controller::DroneController, - input::{Input, InputRecording}, + input::{Input, InputRecording, ModeInput}, Drone, }, engine::World, @@ -161,8 +161,8 @@ impl Simulation { let mut target_angular_vel: na::Vector3 = na::vector![ current_input.joystick.roll_input, - current_input.joystick.yaw_input, current_input.joystick.pitch_input, + current_input.joystick.yaw_input, ] * 3.14; let mut target_mot: na::Vector3 = Default::default(); @@ -172,19 +172,30 @@ impl Simulation { .as_mut_any() .downcast_mut::() { - target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0; + if current_input.mode != ModeInput::Acro { + target_angular_vel = cont.rotation_rt.last_output.unwrap_or_default().0; + } target_mot = cont.rate_rt.last_output.unwrap_or_default().0; } + /* + # roll, pitch, yaw + motor_map = [ + [1.0, -1.0, 1.0], # Front Right + [-1.0, -1.0, -1.0], # Front Left + [-1.0, 1.0, 1.0], # Rear Left + [1.0, 1.0, -1.0], # Rear Right + ] + */ let applied_motor_diff: na::Vector3 = na::vector![ - (self.drone.current_throttles[1] + self.drone.current_throttles[2] - - self.drone.current_throttles[0] - - self.drone.current_throttles[3]), - (self.drone.current_throttles[0] + self.drone.current_throttles[2] + (self.drone.current_throttles[0] + self.drone.current_throttles[3] - self.drone.current_throttles[1] - - self.drone.current_throttles[3]), + - self.drone.current_throttles[2]), (self.drone.current_throttles[2] + self.drone.current_throttles[3] - self.drone.current_throttles[0] - - self.drone.current_throttles[1]) + - self.drone.current_throttles[1]), + (self.drone.current_throttles[0] + self.drone.current_throttles[2] + - self.drone.current_throttles[1] + - self.drone.current_throttles[3]) ] / 2.0; if let Some(logger) = &mut self.logger { @@ -201,8 +212,8 @@ impl Simulation { rot_current: self.drone.get_rot(&self.world).scaled_axis().into(), rot_target: na::UnitQuaternion::from_scaled_axis(vector![ current_input.rotation.roll, - current_input.rotation.yaw, - current_input.rotation.pitch + current_input.rotation.pitch, + current_input.rotation.yaw ]) .scaled_axis() .into(),