Cansat
/
RustPhysicsMQ
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

modules almosttt working

This commit is contained in:
franchioping 2026-02-04 23:01:09 +00:00
parent 4d0b1991b2
commit 79734a5394
33 changed files with 479 additions and 374 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
configurations/all.toml Normal file
View File

@ -0,0 +1,47 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
# The Stack: First layer computes target speed, second layer computes motor torque
layers = [
# { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
{ type = "Rate", max_rate = 3.14, kp = [
0.01,
0.1,
0.01,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
] },
]
# + (map[0] * setpoint.x) // Roll
# + (map[1] * setpoint.y) // Yaw
# + (map[2] * setpoint.z); // Pitch
# /*
# * Motor position indices
# * ^ - Front
# * |
# * |
# * 1 --- 0
# * | |
# * | |
# * 2 --- 3
# */
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
]

47
configurations/pitch.toml Normal file
View File

@ -0,0 +1,47 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
# The Stack: First layer computes target speed, second layer computes motor torque
layers = [
# { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
{ type = "Rate", max_rate = 3.14, kp = [
0.0,
0.0,
0.01,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
] },
]
# + (map[0] * setpoint.x) // Roll
# + (map[1] * setpoint.y) // Yaw
# + (map[2] * setpoint.z); // Pitch
# /*
# * Motor position indices
# * ^ - Front
# * |
# * |
# * 1 --- 0
# * | |
# * | |
# * 2 --- 3
# */
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
]

47
configurations/roll.toml Normal file
View File

@ -0,0 +1,47 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
# The Stack: First layer computes target speed, second layer computes motor torque
layers = [
# { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
{ type = "Rate", max_rate = 3.14, kp = [
0.01,
0.0,
0.0,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
] },
]
# + (map[0] * setpoint.x) // Roll
# + (map[1] * setpoint.y) // Yaw
# + (map[2] * setpoint.z); // Pitch
# /*
# * Motor position indices
# * ^ - Front
# * |
# * |
# * 1 --- 0
# * | |
# * | |
# * 2 --- 3
# */
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
]

14
configurations/sim_std_mot_std_low_01.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.005, 0.005, 0.005]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_02.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.01, 0.01, 0.01]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_03.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.015, 0.015, 0.015]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_04.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.02, 0.02, 0.02]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_05.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.025, 0.025, 0.025]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_06.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.03, 0.03, 0.03]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_07.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.035, 0.035, 0.035]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_08.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.04, 0.04, 0.04]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_09.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.045, 0.045, 0.045]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_std_low_10.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
target_rate = 3.141592
proportional_multiplier = [0.05, 0.05, 0.05]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_01.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.005, 0.005, 0.005]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_02.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.01, 0.01, 0.01]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_03.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.015, 0.015, 0.015]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_04.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.02, 0.02, 0.02]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_05.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.025, 0.025, 0.025]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_06.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.03, 0.03, 0.03]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_07.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.035, 0.035, 0.035]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_08.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.04, 0.04, 0.04]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_09.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.045, 0.045, 0.045]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

14
configurations/sim_std_mot_tc_low_10.toml
View File

@ -1,14 +0,0 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 0.5
mass = 0.350
time_constant=0.01
target_rate = 3.141592
proportional_multiplier = [0.05, 0.05, 0.05]
integral_multiplier = [0.0, 0.0, 0.0]
diferential_multiplier = [0.0, 0.0, 0.0]

47
configurations/yaw.toml Normal file
View File

@ -0,0 +1,47 @@
tickrate = 6000.0
drone_tick_rate = 600
max_thrust = 2.6
max_torque = 2.0
mass = 0.350
# The Stack: First layer computes target speed, second layer computes motor torque
layers = [
# { type = "Angle", max_angle = 0.78, kp = [4.0, 4.0, 4.0], ki = [0.0, 0.0, 0.0], kd = [0.1, 0.1, 0.1] },
{ type = "Rate", max_rate = 3.14, kp = [
0.0,
0.01,
0.0,
], ki = [
0.0,
0.0,
0.0,
], kd = [
0.0,
0.0,
0.0,
] },
]
# + (map[0] * setpoint.x) // Roll
# + (map[1] * setpoint.y) // Yaw
# + (map[2] * setpoint.z); // Pitch
# /*
# * Motor position indices
# * ^ - Front
# * |
# * |
# * 1 --- 0
# * | |
# * | |
# * 2 --- 3
# */
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
]

70
src/config.rs
View File

@ -1,49 +1,59 @@
use rapier3d::prelude::*;
use serde::Deserialize;
#[derive(Debug, serde::Deserialize)]
#[derive(Debug, Deserialize, Clone)]
pub struct PidConfig {
pub kp: [f32; 3],
pub ki: [f32; 3],
pub kd: [f32; 3],
}
#[derive(Debug, Deserialize, Clone)]
#[serde(tag = "type")]
pub enum LayerConfig {
/// Controls angular velocity (Input: joystick/previous layer -> Output: torque)
Rate {
#[serde(flatten)]
pid: PidConfig,
max_rate: f32,
},
/// Controls orientation (Input: joystick -> Output: desired angular velocity)
Angle {
#[serde(flatten)]
pid: PidConfig,
max_angle: f32,
},
}
#[derive(Debug, Deserialize, Clone)]
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],
// --- Modular Controller Stack ---
// The order of this Vec defines the stack (e.g., [Angle, Rate])
pub layers: Vec<LayerConfig>,
// Motors
/// Maps [Pitch, Yaw, Roll] to each of the 4 motors
pub motor_map: [[f32; 3]; 4],
// Motors & Physics
pub max_thrust: f32,
pub max_torque: f32,
#[serde(default)]
pub time_constant: 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]
]
impl PidConfig {
pub fn p_vec(&self) -> Vector<f32> {
vector![self.kp[0], self.kp[1], self.kp[2]]
}
pub fn integral(&self) -> Vector<f32> {
vector![
self.integral_multiplier[0],
self.integral_multiplier[1],
self.integral_multiplier[2]
]
pub fn i_vec(&self) -> Vector<f32> {
vector![self.ki[0], self.ki[1], self.ki[2]]
}
pub fn diferential(&self) -> Vector<f32> {
vector![
self.diferential_multiplier[0],
self.diferential_multiplier[1],
self.diferential_multiplier[2]
]
pub fn d_vec(&self) -> Vector<f32> {
vector![self.kd[0], self.kd[1], self.kd[2]]
}
}

8
src/drone.rs
View File

@ -7,6 +7,7 @@ pub mod controller;
pub mod fpvcontroller;
pub mod input;
pub mod pidcontroller;
pub mod stacked;
use controller::*;
pub use input::JoystickInput;
@ -36,9 +37,9 @@ impl Default for MotorCharacteristics {
*/
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, 0.0, -5.0],
rp::point![-5.0, 0.0, 5.0],
],
max_thrust: 2.6,
max_torque: 0.5,
@ -80,7 +81,8 @@ impl Drone {
* A Poor Man's fluid simulation :D
*/
// .linear_damping(0.2) // Damps velocity slowly
.angular_damping(0.2) // Damps angular velocity slowly
.angular_damping(0.1) // Damps angular velocity slowly
.gyroscopic_forces_enabled(true)
.build(),
);
let width = 0.40;

4
src/drone/pidcontroller.rs
View File

@ -2,9 +2,9 @@
use nalgebra::{self as na, vector};
use std::{any::Any, f32};
use crate::drone::controller::DroneController;
use crate::drone::JoystickInput;
use crate::drone::MotorCharacteristics;
use crate::drone::controller::DroneController;
#[derive(Default)]
pub struct PIDControllerState {
@ -64,7 +64,7 @@ impl PIDController {
return coords;
}
fn get_desired_and_error(&mut self) -> [na::Vector3<f32>; 2] {
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();

221
src/drone/stacked.rs Normal file
View File

@ -0,0 +1,221 @@
#![allow(dead_code)]
use nalgebra::{self as na, vector};
use std::{any::Any, f32};
use crate::drone::controller::DroneController;
use crate::drone::JoystickInput;
use crate::config::*;
pub struct DroneState {
pub rotation: na::UnitQuaternion<f32>,
pub angular_velocity: na::Vector3<f32>,
}
pub enum ControllerModule {
Rate {
processor: PidProcessor,
max_rate: f32,
},
Angle {
processor: PidProcessor,
max_angle: f32,
},
}
impl ControllerModule {
/// Takes the current setpoint (from joystick or previous layer) and
/// returns the command for the next layer.
pub fn process(
&mut self,
setpoint: na::Vector3<f32>,
state: &DroneState,
dt: f32,
is_first_layer: bool,
) -> na::Vector3<f32> {
match self {
ControllerModule::Angle {
processor,
max_angle,
} => {
// Setpoint is -1.0..1.0, scale it to target Radians
let target_angles = setpoint * *max_angle;
let (r, p, y) = state.rotation.euler_angles();
let current_angles = na::vector![r, y, p];
// Output of Angle PID = Desired Angular Velocity
processor.update(target_angles, current_angles, dt)
}
ControllerModule::Rate {
processor,
max_rate,
} => {
// If Rate is the start of the chain (Acro mode), scale the joystick.
// If it's the second layer, the setpoint is already a velocity from the Angle layer.
let target_velocity = if is_first_layer {
setpoint * *max_rate
} else {
setpoint
};
let rot = state.rotation;
let current = rot.inverse().transform_vector(&state.angular_velocity);
// Output of Rate PID = Desired Torque/Correction Force
processor.update(target_velocity, current, dt)
}
}
}
}
pub struct StackedController {
modules: Vec<ControllerModule>,
config: SimulationConfig,
// State
drone_state: DroneState,
input: JoystickInput,
last_time: f32,
current_time: f32,
}
impl StackedController {
pub fn set_input(&mut self, inp: JoystickInput) {
self.input = inp;
}
pub fn new(config: SimulationConfig) -> Self {
let mut modules = Vec::new();
for layer in &config.layers {
match layer {
LayerConfig::Angle { pid, max_angle } => {
modules.push(ControllerModule::Angle {
processor: PidProcessor::new(pid),
max_angle: *max_angle,
});
}
LayerConfig::Rate { pid, max_rate } => {
modules.push(ControllerModule::Rate {
processor: PidProcessor::new(pid),
max_rate: *max_rate,
});
}
}
}
Self {
modules,
config,
input: JoystickInput::default(),
drone_state: DroneState {
rotation: na::UnitQuaternion::identity(),
angular_velocity: na::Vector3::zeros(),
},
last_time: 0.0,
current_time: 0.0,
}
}
}
impl DroneController for StackedController {
fn set_rotation(&mut self, rot: na::UnitQuaternion<f32>) {
self.drone_state.rotation = rot;
}
fn set_angular_velocity(&mut self, vel: na::Vector3<f32>) {
self.drone_state.angular_velocity = vel;
}
fn set_time(&mut self, t: f32) {
self.last_time = self.current_time;
self.current_time = t;
}
fn get_motor_throttles(&mut self) -> [f32; 4] {
let dt = (self.current_time - self.last_time).max(0.0001);
// Input normalized -1.0 to 1.0 from joystick
let mut setpoint = vector![
self.input.roll_input,
self.input.yaw_input,
self.input.pitch_input,
];
// --- CASCADED PROCESSING ---
// If Layer 0 is Angle, setpoint becomes Desired Rate.
// If Layer 1 is Rate, it takes that Desired Rate and outputs Torque.
//
for (i, module) in self.modules.iter_mut().enumerate() {
let is_first_layer = i == 0;
setpoint = module.process(setpoint, &self.drone_state, dt, is_first_layer);
}
// --- MOTOR MIXER ---
let mut motors = [0.0; 4];
for i in 0..4 {
let map = self.config.motor_map[i];
motors[i] = self.input.throttle_input
+ (map[0] * setpoint.x) // Roll
+ (map[1] * setpoint.y) // Yaw
+ (map[2] * setpoint.z); // Pitch
}
// Clamp while keeping relative values
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;
}
}
motors
}
fn as_any(&self) -> &dyn Any {
self
}
fn as_mut_any(&mut self) -> &mut dyn Any {
self
}
}
pub struct PidProcessor {
kp: na::Vector3<f32>,
ki: na::Vector3<f32>,
kd: na::Vector3<f32>,
integral: na::Vector3<f32>,
last_error: na::Vector3<f32>,
}
impl PidProcessor {
fn new(c: &PidConfig) -> Self {
Self {
kp: c.p_vec(),
ki: c.i_vec(),
kd: c.d_vec(),
integral: na::Vector3::zeros(),
last_error: na::Vector3::zeros(),
}
}
fn update(
&mut self,
target: na::Vector3<f32>,
current: na::Vector3<f32>,
dt: f32,
) -> na::Vector3<f32> {
let error = target - current;
self.integral += error * dt;
let derivative = (error - self.last_error) / dt;
self.last_error = error;
error.component_mul(&self.kp)
+ self.integral.component_mul(&self.ki)
+ derivative.component_mul(&self.kd)
}
}

1
src/main.rs
View File

@ -8,6 +8,7 @@ mod camera;
mod drone;
mod rendering;
mod config;
mod graphics_util;
use crate::{drone::fpvcontroller::JoystickInput, rendering::Renderer};

49
src/main_batch.rs
View File

@ -1,5 +1,3 @@
use macroquad::prelude as mq;
mod engine;
use engine::*;
mod camera;
@ -22,48 +20,15 @@ const INPUTS_DIR: &str = "inputs/";
const CONFIGS_DIR: &str = "configurations/";
const RESULTS_DIR: &str = "results/";
fn window_conf() -> mq::Conf {
mq::Conf {
window_title: "RustDroneSim".to_owned(),
window_resizable: true,
platform: mq::miniquad::conf::Platform {
..Default::default()
},
..Default::default()
}
}
use std::env;
use std::path::PathBuf;
use std::thread::JoinHandle;
enum RunMode {
Batch,
Record { output: String },
fn main() {
run_batch();
}
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)]
async fn main() {
match parse_cli() {
RunMode::Batch => run_batch().await,
RunMode::Record { output } => run_record(output).await,
}
}
async fn run_batch() {
fn run_batch() {
let _ = fs::remove_dir_all(RESULTS_DIR);
let _ = fs::create_dir_all(RESULTS_DIR);
@ -107,13 +72,7 @@ fn run(input_path: &PathBuf, config_path: &PathBuf) {
let drone = drone::Drone::new(
&mut world,
Box::new(drone::pidcontroller::PIDController {
target_rate: config.target_rate,
proportional_multiplier: config.proportional(),
integral_multiplier: config.integral(),
diferential_multiplier: config.diferential(),
..Default::default()
}),
Box::new(drone::stacked::StackedController::new(config.clone())),
drone::MotorCharacteristics {
max_thrust: config.max_thrust,
max_torque: config.max_torque,

2
src/rendering.rs
View File

@ -91,8 +91,8 @@ impl MaterialUni {
}
}
pub fn as_type(&self) -> mq::UniformType {
use mq::UniformType::*;
use MaterialUni::*;
use mq::UniformType::*;
match self {
RenderNormalsBool => Int1,
RenderShadowsBool => Int1,

2
src/rendering/ui.rs
View File

@ -1,6 +1,6 @@
use crate::rendering::{self, MaterialUni};
use macroquad::prelude::*;
use macroquad::ui::{hash, Ui};
use macroquad::ui::{Ui, hash};
use strum::IntoEnumIterator;

28
src/simulation.rs
View File

@ -218,7 +218,7 @@ impl Simulation {
.drone
.controller
.as_mut_any()
.downcast_mut::<crate::drone::pidcontroller::PIDController>()
.downcast_mut::<crate::drone::stacked::StackedController>()
{
cont.set_input(current_input);
}
@ -246,18 +246,22 @@ impl Simulation {
throttle + pitch - yaw + roll,
];
*/
let m = self.drone.current_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])
] / 2.0;
} else {
target_angular_vel = na::vector![0.0, 0.0, 0.0];
target_angular_vel = na::vector![
current_input.roll_input,
current_input.yaw_input,
current_input.pitch_input,
] * 3.14;
desired_motor_diff = na::vector![0.0, 0.0, 0.0];
applied_motor_diff = na::vector![0.0, 0.0, 0.0];
}
let m = self.drone.current_throttles;
applied_motor_diff = na::vector![
(m[1] + m[2] - m[0] - m[3]),
(m[0] + m[2] - m[1] - m[3]),
(m[2] + m[3] - m[0] - m[1])
] / 2.0;
self.state.data_results.push(DataResultRecord {
time: self.world.get_time(),
current_angular_velocity: self
@ -282,9 +286,9 @@ impl Simulation {
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"
)?;
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;