diff --git a/Cargo.toml b/Cargo.toml index e0ca986..3fa1eab 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -20,11 +20,6 @@ csv = "1.4.0" serde_with = "3" -[[bin]] -name = "test" -path = "src/main_test.rs" - - # [profile.dev.package.rapier3d] # opt-level = 3 diff --git a/src/drone.rs b/src/drone.rs index 03c3150..f7eb114 100644 --- a/src/drone.rs +++ b/src/drone.rs @@ -4,9 +4,7 @@ use rapier3d::prelude as rp; use crate::engine::{ColliderExtraData, World}; pub mod controller; -pub mod fpvcontroller; pub mod input; -pub mod pidcontroller; pub mod stacked; use controller::*; pub use input::JoystickInput; diff --git a/src/drone/fpvcontroller.rs b/src/drone/fpvcontroller.rs deleted file mode 100644 index 0d80ab4..0000000 --- a/src/drone/fpvcontroller.rs +++ /dev/null @@ -1,80 +0,0 @@ -#![allow(dead_code)] -use std::any::Any; - -use crate::drone::controller::DroneController; - -pub use crate::drone::JoystickInput; - -pub struct FPVController { - input: JoystickInput, - rate_yaw: f32, - rate_pitch: f32, - rate_roll: f32, -} - -impl FPVController { - pub fn set_input(&mut self, input: JoystickInput) { - self.input = input; - } -} - -impl Default for FPVController { - fn default() -> Self { - Self { - input: JoystickInput::default(), - rate_yaw: 1.0, - rate_pitch: 1.0, - rate_roll: 1.0, - } - } -} - -impl DroneController for FPVController { - fn get_motor_throttles(&mut self) -> [f32; 4] { - let yaw = self.rate_yaw * self.input.yaw_input; - let roll = self.rate_roll * self.input.roll_input; - let pitch = self.rate_pitch * self.input.pitch_input; - let throttle = self.input.throttle_input; - - /* - * Motor position indices - * ^ - Front - * | - * | - * 1 --- 0 - * | | - * | | - * 2 --- 3 - */ - - let mut motors = [ - throttle + pitch - roll + yaw, - throttle + pitch + roll - yaw, - throttle - pitch + roll + yaw, - throttle - pitch - roll - yaw, - ]; - // motors.max - let max = motors - .iter() - .copied() - .max_by(|a, b| a.total_cmp(b)) - .unwrap_or(0.0); - if max > 1.0 { - for v in motors.iter_mut() { - *v /= max; - } - } - return motors; - } - - /* - * These should be implemented like this for all the classes that implement DroneController - * Works as an example implementation - */ - fn as_any(&self) -> &dyn Any { - self - } - fn as_mut_any(&mut self) -> &mut dyn Any { - self - } -} diff --git a/src/drone/pidcontroller.rs b/src/drone/pidcontroller.rs deleted file mode 100644 index cb62571..0000000 --- a/src/drone/pidcontroller.rs +++ /dev/null @@ -1,146 +0,0 @@ -#![allow(dead_code)] -use nalgebra::{self as na, vector}; -use std::{any::Any, f32}; - -use crate::drone::JoystickInput; -use crate::drone::MotorCharacteristics; -use crate::drone::controller::DroneController; - -#[derive(Default)] -pub struct PIDControllerState { - last_time: f32, - current_time: f32, - last_rotation: na::Unit>, - current_rotation: na::Unit>, - angular_velocity: na::Vector3, - last_error: na::Vector3, - error_sum: na::Vector3, -} - -impl PIDControllerState { - pub fn dt(&self) -> f32 { - return self.current_time - self.last_time; - } -} - -pub struct PIDController { - pub input: JoystickInput, - pub target_rate: f32, - pub proportional_multiplier: na::Vector3, - pub integral_multiplier: na::Vector3, - pub diferential_multiplier: na::Vector3, - pub multiply_mode: bool, - pub state: PIDControllerState, -} - -impl Default for PIDController { - fn default() -> Self { - Self { - input: JoystickInput::default(), - target_rate: f32::consts::PI, - state: Default::default(), - multiply_mode: false, - proportional_multiplier: vector![0.13, 3.0, 0.13], - integral_multiplier: vector![0.0, 0.0, 0.0], - diferential_multiplier: vector![0.001, 0.01, 0.001], - } - } -} - -impl PIDController { - pub fn set_input(&mut self, input: JoystickInput) { - self.input = input; - } - pub fn get_angular_velocity(&self) -> na::Vector3 { - return self.state.angular_velocity; - } - - pub fn get_desired_angular_velocity(&self) -> na::Vector3 { - let coords = na::Vector3::new( - self.input.roll_input, - self.input.yaw_input, - self.input.pitch_input, - ) * self.target_rate; - return coords; - } - - pub fn get_desired_and_error(&mut self) -> [na::Vector3; 2] { - let rot = self.state.current_rotation; - let current = rot.inverse().transform_vector(&self.get_angular_velocity()); - let target = self.get_desired_angular_velocity(); - - let error: na::Vector3 = target - current; - let error_dif: na::Vector3 = error - self.state.last_error; - - let forces_to_apply: na::Vector3 = 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, error]; - } - - pub fn get_desired_motor_diffs(&mut self) -> na::Vector3 { - return self.get_desired_and_error()[0]; - } -} - -impl DroneController for PIDController { - fn set_rotation(&mut self, rotation: nalgebra::Unit>) { - self.state.last_rotation = self.state.current_rotation; - self.state.current_rotation = rotation; - } - fn set_angular_velocity(&mut self, angvel: nalgebra::Vector3) { - self.state.angular_velocity = angvel; - } - fn set_time(&mut self, time: f32) { - self.state.last_time = self.state.current_time; - self.state.current_time = time; - } - fn set_motor_characteristics(&self, _motor_characteristics: &MotorCharacteristics) {} - fn get_motor_throttles(&mut self) -> [f32; 4] { - let [forces_to_apply, error] = self.get_desired_and_error(); - self.state.error_sum += error * self.state.dt(); - self.state.last_error = error; - - let throttle = self.input.throttle_input; - - let pitch = forces_to_apply.x; - let yaw = forces_to_apply.y; - let roll = forces_to_apply.z; - - let mut motors: [f32; 4] = [ - throttle - pitch + yaw + roll, - throttle - pitch - yaw - roll, - throttle + pitch + yaw - roll, - throttle + pitch - yaw + roll, - ]; - - let max = motors - .iter() - .copied() - .max_by(|a, b| a.total_cmp(b)) - .unwrap_or(0.0); - if max > 1.0 { - for v in motors.iter_mut() { - *v /= max; - } - } - - return motors; - } - - fn as_any(&self) -> &dyn Any { - self - } - fn as_mut_any(&mut self) -> &mut dyn Any { - self - } -} diff --git a/src/drone/stacked/mixer.rs b/src/drone/stacked/mixer.rs index 7cf6d21..dd124fb 100644 --- a/src/drone/stacked/mixer.rs +++ b/src/drone/stacked/mixer.rs @@ -1,14 +1,14 @@ use nalgebra as na; -#[derive(Default)] pub enum MotorMixingMode { ThrottleAuthority, NoTorqueScalling, - ThrottleAuthorityReasonable { - min_scale: f32, - }, - #[default] - AttitudeAuthority, + ThrottleAuthorityReasonable { min_scale: f32 }, +} +impl Default for MotorMixingMode { + fn default() -> Self { + Self::ThrottleAuthorityReasonable { min_scale: 0.1 } + } } pub struct MotorMixer { @@ -33,9 +33,6 @@ impl MotorMixer { .mix_throttle_authority_reasonable(throttle, torque, min_scale) .0; } - AttitudeAuthority => { - return self.mix_attitude_authority(throttle, torque); - } } } @@ -57,8 +54,6 @@ impl MotorMixer { /// Throttle still has authority, this is, torque is limited by the throttle, but within reason, which makes it "reasonable" /// /// Avoids the downside of the previous implementation: At 100% Throttle, Torque would always be 0. This lowers the actual throttle. - /// - /// If min_scale is 1, acts as attitude authority pub fn mix_throttle_authority_reasonable( &self, throttle: f32, @@ -66,6 +61,9 @@ impl MotorMixer { min_scale: f32, ) -> ([f32; 4], bool) { let mut delta = self.compute_delta(torque); + let throttle_range_len = self.max_throttle - self.min_throttle; + + delta.iter_mut().for_each(|x| *x *= 0.25); let mut scale = 1.0f32; @@ -79,10 +77,14 @@ impl MotorMixer { scale = scale.clamp(min_scale, 1.0); - delta.iter_mut().for_each(|x| *x *= scale); + 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 delta_dif = max_delta - min_delta; + + if delta_dif > throttle_range_len { + scale *= throttle_range_len / delta_dif; + } - let max_delta = delta.into_iter().reduce(f32::max).unwrap_or(0.0); - let min_delta = delta.into_iter().reduce(f32::min).unwrap_or(0.0); let lim_throttle = throttle.clamp( self.min_throttle + min_delta.abs(), self.max_throttle - max_delta.abs(), @@ -90,7 +92,7 @@ impl MotorMixer { let mut motors = [0.0f32; 4]; for i in 0..4 { - motors[i] = lim_throttle + delta[i]; + motors[i] = lim_throttle + delta[i] * scale; } let saturated = scale < 1.0; diff --git a/src/main.rs b/src/main.rs index 27147e9..c389583 100644 --- a/src/main.rs +++ b/src/main.rs @@ -18,7 +18,7 @@ use helpers::list_files; use std::fs; const INPUTS_DIR: &str = "inputs/"; -const CONFIGS_DIR: &str = "configurations/"; +const CONFIGS_DIR: &str = "configurations/final/"; const RESULTS_DIR: &str = "results/"; use std::path::PathBuf; @@ -95,23 +95,28 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) { sim.run().unwrap(); } -async fn run_record(output: String) { - println!("Recording inputs to {}", output); +async fn run_record(output: String, config_path: &PathBuf) { + println!( + "Recording inputs to {}, using config {}", + output, + config_path.to_str().unwrap_or("") + ); - let tickrate = 60000.0; - let mut world = World::new(tickrate); + 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::pidcontroller::PIDController { - ..Default::default() - }), + Box::new(drone::stacked::StackedController::new(config.clone())), drone::MotorCharacteristics { - max_thrust: 2.6, - max_torque: 0.5, + max_thrust: config.max_thrust, + max_torque: config.max_torque, + time_constant: config.time_constant, ..Default::default() }, - 0.350, + config.mass, ); let mut sim = Simulation::new( @@ -119,7 +124,7 @@ async fn run_record(output: String) { world, SimMode::Record(InputRecording::default(), output), None, - 600, + config.drone_tick_rate, ); sim.run_and_render().await.unwrap(); diff --git a/src/main_test.rs b/src/main_test.rs deleted file mode 100644 index 06bb4ec..0000000 --- a/src/main_test.rs +++ /dev/null @@ -1,95 +0,0 @@ -use macroquad::prelude as mq; -use rapier3d::prelude::*; - -mod engine; -use engine::*; - -mod camera; -mod drone; -mod rendering; - -mod config; -mod graphics_util; - -use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer}; - -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() - } -} - -#[macroquad::main(window_conf)] -async fn main() { - /* World Setup */ - let mut world = World::new(600.0); - - world.register_free_collider( - ColliderBuilder::cuboid(30.0, 1.0, 30.0) - .translation(vector![0.0, -2.0, 0.0]) - .restitution(0.5) - .build(), - None, - ); - - let mut drone_controller = drone::pidcontroller::PIDController::default(); - drone_controller.set_input(JoystickInput { - throttle_input: 0.2, - yaw_input: 0.2, - roll_input: 0.2, - pitch_input: 0.0, - }); - let mut drone_obj = drone::Drone::new( - &mut world, - Box::new(drone_controller), - drone::MotorCharacteristics { - max_thrust: 2.6, - max_torque: 0.5, - ..Default::default() - }, - 0.350, - ); - - /* Renderer Setup */ - let camera = camera::AttachedCamera::new( - drone_obj.rb_handle, - vector![1.0, 0.0, 0.0], - vector![0.5, 0.0, 0.0], - ); - 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); - - loop { - renderer.update_camera(&world); - - if mq::is_key_pressed(mq::KeyCode::Q) { - break; - } - - // Physics - world.step(); - let _ = clearscreen::clear(); - drone_obj.process_tick(&mut world); - - // Rendering - renderer.draw(&mut world); - - if world.tick % (10) == 0 { - // renderer.update_light(&world); - // world.clear_ofb(); - mq::next_frame().await; - } - } -} diff --git a/src/simulation.rs b/src/simulation.rs index 8743c11..2fed0a1 100644 --- a/src/simulation.rs +++ b/src/simulation.rs @@ -159,13 +159,12 @@ impl Simulation { } self.drone.process_tick(&mut self.world); - let mut target_angular_vel: na::Vector3 = na::vector![ + let target_angular_vel: na::Vector3 = na::vector![ current_input.roll_input, current_input.yaw_input, current_input.pitch_input, ] * 3.14; - let mut desired_motor_diff: na::Vector3 = na::vector![0.0, 0.0, 0.0]; let applied_motor_diff: na::Vector3 = na::vector![ (self.drone.current_throttles[1] + self.drone.current_throttles[2] - self.drone.current_throttles[0] @@ -178,16 +177,6 @@ impl Simulation { - self.drone.current_throttles[1]) ] / 2.0; - if let Some(cont) = self - .drone - .controller - .as_mut_any() - .downcast_mut::() - { - desired_motor_diff = cont.get_desired_motor_diffs(); - target_angular_vel = cont.get_desired_angular_velocity(); - } - if let Some(logger) = &mut self.logger { logger .log(&crate::logger::SimLogRow::from_data( @@ -199,7 +188,7 @@ impl Simulation { .transform_vector(&self.drone.get_angvel(&self.world)) .into(), applied_motor_diff, - desired_motor_diff, + na::vector![0.0, 0.0, 0.0], )) .unwrap(); }