use real PID for stability control

src/action.rs

@@ -7,7 +7,7 @@ use bevy_rapier3d::prelude::{
 };
 
 use crate::{
-    bike::{CyberBikeBody, CyberBikeCollider, CyberBikeControl},
+    bike::{CyberBikeBody, CyberWheel},
     input::InputState,
 };
 
@@ -24,7 +24,7 @@ impl Default for MovementSettings {
         Self {
             sensitivity: 6.0,
             accel: 40.0,
-            gravity: 5.0,
+            gravity: 7.0,
         }
     }
 }
@@ -34,19 +34,47 @@ impl Default for MovementSettings {
 struct CatControllerSettings {
     pub kp: f32,
     pub kd: f32,
-    pub kws: f32,
+    pub ki: f32,
 }
 
 impl Default for CatControllerSettings {
     fn default() -> Self {
         Self {
-            kp: 10.0,
+            kp: 17.0,
             kd: 4.0,
-            kws: 0.85,
+            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;
 }
@@ -58,6 +86,7 @@ fn gravity(
     let (xform, mut forces) = query.single_mut();
     let grav = xform.translation.normalize() * -settings.gravity;
     forces.force = grav;
+    forces.torque = Vec3::ZERO;
 }
 
 fn falling_cat(
@@ -66,25 +95,38 @@ fn falling_cat(
     settings: Res<CatControllerSettings>,
 ) {
     let (xform, mut forces, mut control_vars) = bike_query.single_mut();
-    let up = xform.translation.normalize();
+    let wup = xform.translation.normalize();
     let bike_up = xform.up();
 
-    let wright = xform.right().cross(up);
+    let wright = xform.forward().cross(wup).normalize();
-    let wback = wright.cross(up);
+    //let wback = wright.cross(wup);
 
-    let torque = bike_up.cross(up).normalize_or_zero();
+    let roll_error = wright.dot(bike_up);
-    let cos = up.dot(bike_up);
+    let pitch_error = wup.dot(xform.back());
-    let cos = if cos.is_finite() { cos } else { 1.0 };
 
-    let error = 1.0 - cos;
+    // 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 derivative = error - control_vars.prev_error;
+        let mag =
-    control_vars.prev_error = error;
+            (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
-    // this integral term is not an integral, it's more like a weighted moving sum
+        forces.torque += xform.back() * mag;
-    let weighted_sum = control_vars.error_sum + error;
+    };
-    control_vars.error_sum = weighted_sum * 0.8;
 
-    let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative);
+    // 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
@@ -93,32 +135,32 @@ fn falling_cat(
         .map(|x| x as u64)
     {
         if count % 30 == 0 {
-            dbg!(&control_vars, mag, cos, derivative);
+            dbg!(roll_error, pitch_error, &control_vars);
         }
     }
-
+    control_vars.decay();
-    forces.torque = torque * mag;
 }
 
 fn input_forces(
     settings: Res<MovementSettings>,
     input: Res<InputState>,
-    mut cquery: Query<&mut Friction, With<CyberBikeCollider>>,
+    mut cquery: Query<&mut Friction, With<CyberWheel>>,
     mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
 ) {
     let (xform, mut forces) = bquery.single_mut();
-    let mut friction = cquery.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() + Vec3::new(0.5, 0.0, 0.0);
+    let point = xform.translation + xform.forward();
     let force = ExternalForce::at_point(force, point, xform.translation);
     *forces += force;
 }

src/bike.rs

@@ -10,7 +10,7 @@ use bevy_rapier3d::{
     },
 };
 
-use crate::planet::PLANET_RADIUS;
+use crate::{action::CyberBikeControl, planet::PLANET_RADIUS};
 
 type Meshterial<'a> = (
     ResMut<'a, Assets<Mesh>>,
@@ -20,18 +20,9 @@ type Meshterial<'a> = (
 #[derive(Component)]
 pub struct CyberBikeBody;
 
-#[derive(Component)]
-pub struct CyberBikeCollider;
-
 #[derive(Debug, Component)]
 pub struct CyberWheel;
 
-#[derive(Component, Debug, Default, Clone, Copy)]
-pub struct CyberBikeControl {
-    pub error_sum: f32,
-    pub prev_error: f32,
-}
-
 #[derive(Resource, Reflect)]
 #[reflect(Resource)]
 pub struct WheelConfig {
@@ -79,7 +70,7 @@ fn spawn_cyberbike(
     xform.translation += right;
 
     let damping = Damping {
-        angular_damping: 0.5,
+        angular_damping: 2.0,
         linear_damping: 0.1,
     };
     let not_sleeping = Sleeping::disabled();
@@ -129,7 +120,6 @@ fn spawn_cyberbike(
         })
         .insert(Velocity::zero())
         .insert(ExternalForce::default())
-        .insert(CyberBikeCollider)
         .with_children(|rider| {
             rider.spawn(SceneBundle {
                 scene,
@@ -202,6 +192,11 @@ fn spawn_tires(
         ..Default::default()
     };
 
+    let friction = Friction {
+        coefficient: 0.0,
+        ..Default::default()
+    };
+
     let mut wheel_poses = Vec::with_capacity(2);
 
     // front
@@ -228,7 +223,7 @@ fn spawn_tires(
             ..Default::default()
         };
         let wheel_collider = Collider::ball(wheel_rad);
-        let mass_props = ColliderMassProperties::Density(0.001);
+        let mass_props = ColliderMassProperties::Density(0.1);
 
         let damping = conf.damping;
         let prismatic = PrismaticJointBuilder::new(Vec3::Y)
@@ -258,6 +253,8 @@ fn spawn_tires(
                 not_sleeping,
                 joint,
                 wheels_collision_group,
+                friction,
+                CyberWheel,
             ))
             .with_children(|wheel| {
                 wheel.spawn(tire_spundle).insert(pbr_bundle.clone()).insert(
@@ -270,8 +267,7 @@ fn spawn_tires(
             .insert(TransformInterpolation {
                 start: None,
                 end: None,
-            })
+            });
-            .insert(CyberWheel);
     }
 }