From 139b8af742f1bdba7a4f9f511ce127ae5adfa3c7 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Mon, 14 Mar 2022 23:58:58 -0700
Subject: [PATCH] Add PID roll stabilizer.

---
 src/action.rs | 45 ++++++++++++++++++++++++++-------------------
 src/bike.rs   | 17 ++++++++++++-----
 2 files changed, 38 insertions(+), 24 deletions(-)

diff --git a/src/action.rs b/src/action.rs
index 6c1a799..049a559 100644
--- a/src/action.rs
+++ b/src/action.rs
@@ -2,7 +2,7 @@ use bevy::prelude::*;
 use bevy_rapier3d::{na::Vector3, prelude::*};
 
 use crate::{
-    bike::{CyberBikeBody, CyberBikeCollider},
+    bike::{CyberBikeBody, CyberBikeCollider, CyberBikeControl},
     input::InputState,
 };
 
@@ -31,28 +31,35 @@ fn gravity(
     config.gravity = gravity.into();
 }
 
-fn falling_cat(
-    mut bike_query: Query<(&Transform, &mut RigidBodyForcesComponent), With<CyberBikeBody>>,
+fn falling_cat_pid(
+    mut bike_query: Query<(
+        &Transform,
+        &mut RigidBodyForcesComponent,
+        &mut CyberBikeControl,
+    )>,
 ) {
-    let (bike_xform, mut forces) = bike_query.single_mut();
-    let up = bike_xform.translation.normalize();
-    let cam_up = bike_xform.up();
-    let cos = up.dot(cam_up);
+    let (xform, mut forces, mut control_vars) = bike_query.single_mut();
+    let up = xform.translation.normalize();
+    let cam_right = xform.right();
+    let cos = up.dot(cam_right);
+    let error = if cos.is_normal() { -cos } else { 0.0 };
 
-    let roll_cos = up.dot(bike_xform.right());
+    let derivative = error - control_vars.previous;
+    control_vars.previous = error;
 
-    let r_mag = if roll_cos.is_normal() {
-        roll_cos.powi(3) * 8.0
-    } else {
-        0.0
-    };
-    let r_tork = bike_xform.forward() * r_mag;
+    let integral = (control_vars.sum * 0.6) + error;
+    control_vars.sum = integral;
 
-    let theta = cos.acos();
-    let force_mag = if !theta.is_normal() { 0.0 } else { theta * 7.0 };
-    let torque = cam_up.cross(up).normalize() * force_mag;
+    let proportional = error;
 
-    forces.torque = (r_tork + torque).into();
+    let kp = 7.0;
+    let ki = 0.19;
+    let kd = 4.4;
+    let mag = (kp * proportional) + (ki * integral) + (kd * derivative);
+
+    let torque = xform.back() * mag;
+
+    forces.torque = torque.into();
 }
 
 fn drag(
@@ -96,7 +103,7 @@ impl Plugin for CyberActionPlugin {
         app.init_resource::<MovementSettings>()
             .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
             .add_system(gravity)
-            .add_system(falling_cat.label("cat"))
+            .add_system(falling_cat_pid.label("cat"))
             .add_system(drag.label("drag").after("iforces"))
             .add_system(input_forces.label("iforces").after("cat"));
     }
diff --git a/src/bike.rs b/src/bike.rs
index 21af96f..143abf3 100644
--- a/src/bike.rs
+++ b/src/bike.rs
@@ -1,4 +1,4 @@
-use bevy::prelude::*;
+use bevy::{prelude::*, utils::Instant};
 use bevy_rapier3d::prelude::*;
 
 use crate::planet::PLANET_RADIUS;
@@ -14,6 +14,12 @@ pub struct CyberBikeCollider;
 #[derive(Component, Debug)]
 pub struct CyberBikeModel;
 
+#[derive(Component, Debug, Default, Clone, Copy)]
+pub struct CyberBikeControl {
+    pub sum: f32,
+    pub previous: f32,
+}
+
 const BIKE_BODY_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01);
 const BIKE_WHEEL_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10);
 
@@ -78,6 +84,7 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
         })
         .insert(CyberBikeModel)
         .insert(CyberBikeBody)
+        .insert(CyberBikeControl::default())
         .id();
 
     // seriously the cyberbike is so fucking huge what the heck
@@ -86,9 +93,9 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
 
     for (i, &wheel_z) in wheel_positions.iter().enumerate() {
         let (wheel_x, wheel_rad, stiffness) = match i {
-            0 => (-2.6, 0.95, 0.014),
-            2 => (2.6, 0.95, 0.014),
-            1 => (0.0, 1.2, 0.019),
+            0 => (-2.6, 1.0, 0.016),
+            2 => (2.6, 1.0, 0.016),
+            1 => (0.0, 1.2, 0.020),
             _ => unreachable!(),
         };
         let offset = Vec3::new(wheel_x, wheel_y, wheel_z);
@@ -127,7 +134,7 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
             .insert(ColliderDebugRender::from(Color::YELLOW))
             .id();
 
-        let damping = 0.25;
+        let damping = 0.3;
 
         let prismatic = PrismaticJoint::new(Vector::y_axis())
             .local_anchor1(offset.into())