break up action module, actually do real for real PID

src/action.rs

@@ -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<RapierConfiguration>) {
-    config.gravity = Vec3::ZERO;
-}
-
-fn gravity(
-    mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
-    settings: Res<MovementSettings>,
-) {
-    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<Diagnostics>,
-    settings: Res<CatControllerSettings>,
-) {
-    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<MovementSettings>,
-    input: Res<InputState>,
-    mut cquery: Query<&mut Friction, With<CyberWheel>>,
-    mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
-) {
-    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<CyberBikeBody>>) {
-    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::<MovementSettings>()
-            .register_type::<MovementSettings>()
-            .init_resource::<CatControllerSettings>()
-            .register_type::<CatControllerSettings>()
-            .add_plugin(RapierPhysicsPlugin::<NoUserData>::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"));
-    }
-}

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;
+    }
+}

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::<MovementSettings>()
+            .register_type::<MovementSettings>()
+            .init_resource::<CatControllerSettings>()
+            .init_resource::<ActionDebugInstant>()
+            .register_type::<CatControllerSettings>()
+            .add_plugin(RapierPhysicsPlugin::<NoUserData>::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"));
+    }
+}

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<RapierConfiguration>) {
+    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<CyberBikeBody>>,
+    settings: Res<MovementSettings>,
+) {
+    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<Time>,
+    settings: Res<CatControllerSettings>,
+    #[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
+) {
+    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 roll_error = wright.dot(bike_up);
+    let pitch_error = wup.dot(xform.back());
+
+    // only try to correct roll if we're not totally vertical
+    if pitch_error.abs() < 0.8 {
+        let (derivative, integral) = control_vars.update_roll(roll_error, time.delta_seconds());
+        let mag =
+            (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
+        if mag.is_finite() {
+            forces.torque += xform.back() * mag;
+        }
+    }
+
+    // we can do pitch correction any time, it's not coupled to roll
+    let (derivative, integral) = control_vars.update_pitch(pitch_error, time.delta_seconds());
+    let mag = (settings.kp * pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
+    if mag.is_finite() {
+        forces.torque += wright * mag * 0.2;
+    }
+
+    #[cfg(feature = "inspector")]
+    {
+        let now = Instant::now();
+        if (now - debug_instant.0).as_millis() > 500 {
+            dbg!(roll_error, pitch_error, &control_vars);
+            debug_instant.0 = now;
+        }
+    }
+    control_vars.decay();
+}
+
+/// Apply forces to the cyberbike as a result of input.
+pub(crate) fn input_forces(
+    settings: Res<MovementSettings>,
+    input: Res<InputState>,
+    mut cquery: Query<&mut Friction, With<CyberWheel>>,
+    mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
+) {
+    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;
+}
+
+/// General velocity-based drag-force calculation; does not take orientation
+/// into account.
+pub(crate) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
+    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);
+    }
+}

src/bike.rs

@@ -10,7 +10,7 @@ use bevy_rapier3d::{
     },
 };
 
-use crate::{action::CyberBikeControl, planet::PLANET_RADIUS};
+use crate::{action::CatControllerState, planet::PLANET_RADIUS};
 
 type Meshterial<'a> = (
     ResMut<'a, Assets<Mesh>>,
@@ -127,7 +127,7 @@ fn spawn_cyberbike(
             });
         })
         .insert(CyberBikeBody)
-        .insert(CyberBikeControl::default())
+        .insert(CatControllerState::default())
         .id();
 
     spawn_tires(&mut commands, &xform, bike, &wheel_conf, &mut meshterials);