fix mixer? setpoint values that are too high no longer cause crashes
This commit is contained in:
franchioping
parent
26d25f5cbb
commit
249f319309
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
}
|
||||
|
|
@ -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<na::Quaternion<f32>>,
|
||||
current_rotation: na::Unit<na::Quaternion<f32>>,
|
||||
angular_velocity: na::Vector3<f32>,
|
||||
last_error: na::Vector3<f32>,
|
||||
error_sum: na::Vector3<f32>,
|
||||
}
|
||||
|
||||
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<f32>,
|
||||
pub integral_multiplier: na::Vector3<f32>,
|
||||
pub diferential_multiplier: na::Vector3<f32>,
|
||||
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<f32> {
|
||||
return self.state.angular_velocity;
|
||||
}
|
||||
|
||||
pub fn get_desired_angular_velocity(&self) -> na::Vector3<f32> {
|
||||
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<f32>; 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<f32> = target - current;
|
||||
let error_dif: na::Vector3<f32> = error - self.state.last_error;
|
||||
|
||||
let forces_to_apply: na::Vector3<f32> = 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<f32> {
|
||||
return self.get_desired_and_error()[0];
|
||||
}
|
||||
}
|
||||
|
||||
impl DroneController for PIDController {
|
||||
fn set_rotation(&mut self, rotation: nalgebra::Unit<nalgebra::Quaternion<f32>>) {
|
||||
self.state.last_rotation = self.state.current_rotation;
|
||||
self.state.current_rotation = rotation;
|
||||
}
|
||||
fn set_angular_velocity(&mut self, angvel: nalgebra::Vector3<f32>) {
|
||||
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
|
||||
}
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
|
|
|||
29
src/main.rs
29
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("<INVALID CONFIG PATH>")
|
||||
);
|
||||
|
||||
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();
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -159,13 +159,12 @@ impl Simulation {
|
|||
}
|
||||
self.drone.process_tick(&mut self.world);
|
||||
|
||||
let mut target_angular_vel: na::Vector3<f32> = na::vector![
|
||||
let target_angular_vel: na::Vector3<f32> = na::vector![
|
||||
current_input.roll_input,
|
||||
current_input.yaw_input,
|
||||
current_input.pitch_input,
|
||||
] * 3.14;
|
||||
|
||||
let mut desired_motor_diff: na::Vector3<f32> = na::vector![0.0, 0.0, 0.0];
|
||||
let applied_motor_diff: na::Vector3<f32> = 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::<crate::drone::pidcontroller::PIDController>()
|
||||
{
|
||||
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();
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue