From 79b2d4b1754995c7bb1a0c7caa36048a98597e6b Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 29 Jan 2023 17:02:16 -0800 Subject: [PATCH] break up action module, actually do real for real PID --- src/action.rs | 192 --------------------------------------- src/action/components.rs | 108 ++++++++++++++++++++++ src/action/mod.rs | 29 ++++++ src/action/systems.rs | 109 ++++++++++++++++++++++ src/bike.rs | 4 +- 5 files changed, 248 insertions(+), 194 deletions(-) delete mode 100644 src/action.rs create mode 100644 src/action/components.rs create mode 100644 src/action/mod.rs create mode 100644 src/action/systems.rs diff --git a/src/action.rs b/src/action.rs deleted file mode 100644 index eba0631..0000000 --- a/src/action.rs +++ /dev/null @@ -1,192 +0,0 @@ -use bevy::{ - diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin}, - prelude::*, -}; -use bevy_rapier3d::prelude::{ - ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity, -}; - -use crate::{ - bike::{CyberBikeBody, CyberWheel}, - input::InputState, -}; - -#[derive(Debug, Resource, Reflect)] -#[reflect(Resource)] -pub struct MovementSettings { - pub accel: f32, - pub gravity: f32, - pub sensitivity: f32, -} - -impl Default for MovementSettings { - fn default() -> Self { - Self { - sensitivity: 6.0, - accel: 40.0, - gravity: 7.0, - } - } -} - -#[derive(Debug, Resource, Reflect)] -#[reflect(Resource)] -struct CatControllerSettings { - pub kp: f32, - pub kd: f32, - pub ki: f32, -} - -impl Default for CatControllerSettings { - fn default() -> Self { - Self { - kp: 17.0, - kd: 4.0, - ki: 0.05, - } - } -} - -#[derive(Component, Debug, Clone, Copy)] -pub struct CyberBikeControl { - pub roll_integral: f32, - pub roll_prev: f32, - pub pitch_integral: f32, - pub pitch_prev: f32, - decay_factor: f32, -} - -impl Default for CyberBikeControl { - fn default() -> Self { - Self { - roll_integral: Default::default(), - roll_prev: Default::default(), - pitch_integral: Default::default(), - pitch_prev: Default::default(), - decay_factor: 0.9, - } - } -} - -impl CyberBikeControl { - fn decay(&mut self) { - self.roll_integral *= self.decay_factor; - self.pitch_integral *= self.decay_factor; - } -} - -fn zero_gravity(mut config: ResMut) { - config.gravity = Vec3::ZERO; -} - -fn gravity( - mut query: Query<(&Transform, &mut ExternalForce), With>, - settings: Res, -) { - let (xform, mut forces) = query.single_mut(); - let grav = xform.translation.normalize() * -settings.gravity; - forces.force = grav; - forces.torque = Vec3::ZERO; -} - -fn falling_cat( - mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>, - _diagnostics: Res, - settings: Res, -) { - let (xform, mut forces, mut control_vars) = bike_query.single_mut(); - let wup = xform.translation.normalize(); - let bike_up = xform.up(); - - let wright = xform.forward().cross(wup).normalize(); - //let wback = wright.cross(wup); - - let roll_error = wright.dot(bike_up); - let pitch_error = wup.dot(xform.back()); - - // roll - if pitch_error.abs() < 0.8 { - let derivative = roll_error - control_vars.roll_prev; - control_vars.roll_prev = roll_error; - let integral = control_vars.roll_integral + roll_error; - control_vars.roll_integral = integral.min(2.0).max(-2.0); - - let mag = - (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative); - forces.torque += xform.back() * mag; - }; - - // pitch - if roll_error.abs() < 0.5 { - let derivative = pitch_error - control_vars.pitch_prev; - control_vars.pitch_prev = pitch_error; - let integral = control_vars.pitch_integral + pitch_error; - control_vars.pitch_integral = integral.min(1.1).max(-1.1); - - let mag = - (settings.kp * pitch_error) + (settings.ki * integral) + (settings.kd * derivative); - forces.torque += wright * mag * 0.6; - }; - - #[cfg(feature = "inspector")] - if let Some(count) = _diagnostics - .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT) - .and_then(|d| d.smoothed()) - .map(|x| x as u64) - { - if count % 30 == 0 { - dbg!(roll_error, pitch_error, &control_vars); - } - } - control_vars.decay(); -} - -fn input_forces( - settings: Res, - input: Res, - mut cquery: Query<&mut Friction, With>, - mut bquery: Query<(&Transform, &mut ExternalForce), With>, -) { - let (xform, mut forces) = bquery.single_mut(); - - // thrust - let thrust = xform.forward() * input.throttle * settings.accel; - forces.force += thrust; - - // brake - for mut friction in cquery.iter_mut() { - friction.coefficient = if input.brake { 2.0 } else { 0.0 }; - } - - // steering - let force = xform.right() * input.yaw * settings.sensitivity; - let point = xform.translation + xform.forward(); - let force = ExternalForce::at_point(force, point, xform.translation); - *forces += force; -} - -fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With>) { - let (vels, mut forces) = query.single_mut(); - - if let Some(vel) = vels.linvel.try_normalize() { - let v2 = vels.linvel.length_squared(); - forces.force -= vel * (v2 * 0.02); - } -} - -pub struct CyberActionPlugin; -impl Plugin for CyberActionPlugin { - fn build(&self, app: &mut App) { - app.init_resource::() - .register_type::() - .init_resource::() - .register_type::() - .add_plugin(RapierPhysicsPlugin::::default()) - .add_plugin(FrameTimeDiagnosticsPlugin::default()) - .add_startup_system(zero_gravity) - .add_system(gravity.before("cat")) - .add_system(falling_cat.label("cat")) - .add_system(input_forces.label("iforces").after("cat")) - .add_system(drag.label("drag").after("iforces")); - } -} diff --git a/src/action/components.rs b/src/action/components.rs new file mode 100644 index 0000000..f16fcf9 --- /dev/null +++ b/src/action/components.rs @@ -0,0 +1,108 @@ +use std::time::Instant; + +use bevy::{ + prelude::{Component, ReflectResource, Resource}, + reflect::Reflect, +}; + +#[derive(Debug, Resource)] +pub(crate) struct ActionDebugInstant(pub Instant); + +impl Default for ActionDebugInstant { + fn default() -> Self { + ActionDebugInstant(Instant::now()) + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct MovementSettings { + pub accel: f32, + pub gravity: f32, + pub sensitivity: f32, +} + +impl Default for MovementSettings { + fn default() -> Self { + Self { + sensitivity: 6.0, + accel: 40.0, + gravity: 7.0, + } + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct CatControllerSettings { + pub kp: f32, + pub kd: f32, + pub ki: f32, +} + +impl Default for CatControllerSettings { + fn default() -> Self { + Self { + kp: 17.0, + kd: 4.0, + ki: 0.05, + } + } +} + +#[derive(Component, Debug, Clone, Copy)] +pub struct CatControllerState { + pub roll_integral: f32, + pub roll_prev: f32, + pub pitch_integral: f32, + pub pitch_prev: f32, + decay_factor: f32, + roll_limit: f32, + pitch_limit: f32, +} + +impl Default for CatControllerState { + fn default() -> Self { + Self { + roll_integral: Default::default(), + roll_prev: Default::default(), + pitch_integral: Default::default(), + pitch_prev: Default::default(), + decay_factor: 0.9, + roll_limit: 1.5, + pitch_limit: 1.2, + } + } +} + +impl CatControllerState { + pub fn decay(&mut self) { + if self.roll_integral.abs() > 0.1 { + self.roll_integral *= self.decay_factor; + } + if self.pitch_integral.abs() > 0.1 { + self.pitch_integral *= self.decay_factor; + } + } + + pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) { + let lim = self.roll_limit; + self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim); + let derivative = (error - self.roll_prev) / dt; + self.roll_prev = error; + (derivative, self.roll_integral) + } + + pub fn update_pitch(&mut self, error: f32, dt: f32) -> (f32, f32) { + let lim = self.pitch_limit; + self.pitch_integral = (self.pitch_integral + (error * dt)).min(lim).max(-lim); + let derivative = (error - self.pitch_prev) / dt; + self.pitch_prev = error; + (derivative, self.pitch_integral) + } + + pub fn set_limits(&mut self, roll: f32, pitch: f32) { + self.roll_limit = roll; + self.pitch_limit = pitch; + } +} diff --git a/src/action/mod.rs b/src/action/mod.rs new file mode 100644 index 0000000..8646ba7 --- /dev/null +++ b/src/action/mod.rs @@ -0,0 +1,29 @@ +use bevy::{ + diagnostic::FrameTimeDiagnosticsPlugin, + prelude::{App, IntoSystemDescriptor, Plugin}, +}; +use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin}; + +mod components; +mod systems; + +pub use components::*; +use systems::*; + +pub struct CyberActionPlugin; +impl Plugin for CyberActionPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .register_type::() + .init_resource::() + .init_resource::() + .register_type::() + .add_plugin(RapierPhysicsPlugin::::default()) + .add_plugin(FrameTimeDiagnosticsPlugin::default()) + .add_startup_system(zero_gravity) + .add_system(gravity.before("cat")) + .add_system(falling_cat.label("cat")) + .add_system(input_forces.label("iforces").after("cat")) + .add_system(drag.label("drag").after("iforces")); + } +} diff --git a/src/action/systems.rs b/src/action/systems.rs new file mode 100644 index 0000000..29b5d8b --- /dev/null +++ b/src/action/systems.rs @@ -0,0 +1,109 @@ +#[cfg(feature = "inspector")] +use std::time::Instant; + +use bevy::prelude::{Query, Res, ResMut, Time, Transform, Vec3, With}; +use bevy_rapier3d::prelude::{ExternalForce, Friction, RapierConfiguration, Velocity}; + +#[cfg(feature = "inspector")] +use super::ActionDebugInstant; +use super::{CatControllerSettings, CatControllerState, MovementSettings}; +use crate::{ + bike::{CyberBikeBody, CyberWheel}, + input::InputState, +}; + +/// Disable gravity in Rapier. +pub(crate) fn zero_gravity(mut config: ResMut) { + config.gravity = Vec3::ZERO; +} + +/// The gravity vector points from the cyberbike to the center of the planet. +pub(crate) fn gravity( + mut query: Query<(&Transform, &mut ExternalForce), With>, + settings: Res, +) { + let (xform, mut forces) = query.single_mut(); + let grav = xform.translation.normalize() * -settings.gravity; + forces.force = grav; + forces.torque = Vec3::ZERO; +} + +/// PID-based controller for stabilizing attitude; keeps the cyberbike upright. +pub(crate) fn falling_cat( + mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>, + time: Res