From 3e9b079b580c3a9c96fe80e60f907c04916cf356 Mon Sep 17 00:00:00 2001
From: Joe Ardent <code@ardent.nebcorp.com>
Date: Tue, 24 Jan 2023 19:50:33 -0800
Subject: [PATCH] checkpoint

---
 src/action.rs | 29 +++++++++++++++++++----------
 1 file changed, 19 insertions(+), 10 deletions(-)

diff --git a/src/action.rs b/src/action.rs
index 443b394..4f9d64d 100644
--- a/src/action.rs
+++ b/src/action.rs
@@ -28,6 +28,14 @@ impl Default for MovementSettings {
     }
 }
 
+#[derive(Debug, Default, Resource, Reflect)]
+#[reflect(Resource)]
+struct CatControllerSettings {
+    pub kp: f32,
+    pub kd: f32,
+    pub kws: f32,
+}
+
 fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
     config.gravity = Vec3::ZERO;
 }
@@ -41,9 +49,10 @@ fn gravity(
     forces.force = grav;
 }
 
-fn falling_cat_pid(
+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 up = Vec3::Y;
@@ -51,19 +60,17 @@ fn falling_cat_pid(
 
     let torque = bike_up.cross(up).normalize_or_zero();
     let cos = up.dot(bike_up);
-    let cos = if cos.is_finite() { cos } else { 0.0 };
+    let cos = if cos.is_finite() { cos } else { 1.0 };
 
     let error = 1.0 - cos;
+
     let derivative = error - control_vars.prev_error;
     control_vars.prev_error = error;
     // this integral term is not an integral, it's more like a weighted moving sum
-    let integral = (control_vars.error_sum + error) * 0.8;
-    control_vars.error_sum = integral; //.min(2.0).max(-2.0);
+    let weighted_sum = control_vars.error_sum + error;
+    control_vars.error_sum = weighted_sum * 0.8;
 
-    let kp = 13.1;
-    let ki = 1.1;
-    let kd = 8.1;
-    let mag = (kp * error) + (ki * integral) + (kd * derivative);
+    let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative);
 
     if let Some(count) = diagnostics
         .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
@@ -71,7 +78,7 @@ fn falling_cat_pid(
         .map(|x| x as u64)
     {
         if count % 30 == 0 {
-            dbg!(&control_vars, mag);
+            dbg!(&control_vars, mag, cos);
         }
     }
 
@@ -112,11 +119,13 @@ pub struct CyberActionPlugin;
 impl Plugin for CyberActionPlugin {
     fn build(&self, app: &mut App) {
         app.init_resource::<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_pid.label("cat"))
+            .add_system(falling_cat.label("cat"))
             .add_system(input_forces.label("iforces").after("cat"))
             .add_system(drag.label("drag").after("iforces"));
     }