fix PID code, fuckin' with shit still.

src/action.rs

@@ -1,6 +1,12 @@
-use bevy::prelude::*;
-use bevy_rapier3d::prelude::{
-    ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity,
+use bevy::{
+    diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin},
+    prelude::*,
+};
+use bevy_rapier3d::{
+    prelude::{
+        ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity,
+    },
+    render::{DebugRenderMode, RapierDebugRenderPlugin},
 };
 
 use crate::{
@@ -41,76 +47,42 @@ fn gravity(
 fn falling_cat_pid(
     mut bike_query: Query<(
         &Transform,
-        &Velocity,
+        //&Velocity,
         &mut ExternalForce,
         &mut CyberBikeControl,
     )>,
+    diagnostics: Res<Diagnostics>,
 ) {
-    let (xform, vels, mut forces, mut control_vars) = bike_query.single_mut();
+    let (xform, mut forces, mut control_vars) = bike_query.single_mut();
     let up = xform.translation.normalize();
-    let cam_forward = xform.forward();
-    let cam_right = xform.right();
-    let pitch_cos = up.dot(cam_forward);
-    let roll_cos = up.dot(cam_right);
+    let bike_up = xform.up();
 
-    let mut torque = Vec3::ZERO;
+    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 };
 
-    // roll
+    let error = 1.0 - cos;
+    let derivative = error - control_vars.prev_error;
+    control_vars.prev_error = error;
+    let integral = (control_vars.error_sum * 0.8) + error;
+    control_vars.error_sum = integral;
+
+    let kp = 13.1;
+    let ki = 6.0;
+    let kd = 13.1;
+    let mag = (kp * error) + (ki * integral) + (kd * derivative);
+
+    if let Some(count) = diagnostics
+        .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
+        .and_then(|d| d.smoothed())
+        .map(|x| x as u128)
     {
-        let roll_error = if roll_cos.is_normal() { -roll_cos } else { 0.0 };
-        let derivative = roll_error - control_vars.prev_roll_error;
-        control_vars.prev_roll_error = roll_error;
-
-        let integral = (control_vars.roll_sum * 0.8) + roll_error;
-        control_vars.roll_sum = integral;
-
-        //dbg!(integral);
-
-        let proportional = roll_error;
-
-        let kp = 7.1;
-        let ki = 0.8;
-        let kd = 5.1;
-        let mag = (kp * proportional) + (ki * integral) + (kd * derivative);
-
-        let angvel = vels.angvel.as_ref();
-        let roll_rate = cam_forward.dot(Vec3::from_slice(angvel)).abs();
-        //dbg!(roll_rate);
-        if pitch_cos.abs() < 0.85 && roll_rate < 1.0 {
-            torque += xform.back() * mag;
+        if count % 30 == 0 {
+            dbg!(&control_vars, mag);
         }
     }
 
-    // pitch
-    {
-        let pitch_error = if pitch_cos.is_normal() {
-            -pitch_cos
-        } else {
-            0.0
-        };
-
-        let derivative = pitch_error - control_vars.prev_pitch_error;
-        control_vars.prev_pitch_error = pitch_error;
-
-        let integral = (control_vars.pitch_sum * 0.7) + pitch_error;
-        control_vars.pitch_sum = integral;
-
-        let proportional = pitch_error;
-
-        let kp = 7.1;
-        let ki = 0.3;
-        let kd = 4.1;
-        let mag = (kp * proportional) + (ki * integral) + (kd * derivative);
-
-        let angvel = vels.angvel.as_ref();
-        let pitch_rate = cam_right.dot(Vec3::from_slice(angvel)).abs();
-
-        if roll_cos.abs() < 0.85 && pitch_rate < 2.0 {
-            torque += xform.left() * mag;
-        }
-    }
-
-    forces.torque = torque;
+    forces.torque = torque * mag;
 }
 
 fn input_forces(
@@ -130,7 +102,7 @@ fn input_forces(
     friction.coefficient = if input.brake { 2.0 } else { 0.0 };
 
     // steering
-    let torque = xform.up() * input.yaw * settings.sensitivity;
+    let torque = xform.down() * input.yaw * settings.sensitivity;
     forces.torque += torque;
 
     //dbg!(&input);
@@ -150,8 +122,20 @@ fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>)
 pub struct CyberActionPlugin;
 impl Plugin for CyberActionPlugin {
     fn build(&self, app: &mut App) {
+        let mut mode = DebugRenderMode::RIGID_BODY_AXES;
+        //mode.insert(DebugRenderMode::COLLIDER_SHAPES);
+        mode.insert(DebugRenderMode::CONTACTS);
+        mode.insert(DebugRenderMode::JOINTS);
+        let rplugin = RapierDebugRenderPlugin {
+            always_on_top: true,
+            enabled: true,
+            mode,
+            ..Default::default()
+        };
         app.init_resource::<MovementSettings>()
             .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
+            .add_plugin(rplugin)
+            .add_plugin(FrameTimeDiagnosticsPlugin::default())
             .add_startup_system(zero_gravity)
             .add_system(gravity.before("cat"))
             .add_system(falling_cat_pid.label("cat"))

src/bike.rs

@@ -16,10 +16,8 @@ pub struct CyberBikeModel;
 
 #[derive(Component, Debug, Default, Clone, Copy)]
 pub struct CyberBikeControl {
-    pub roll_sum: f32,
-    pub prev_roll_error: f32,
-    pub pitch_sum: f32,
-    pub prev_pitch_error: f32,
+    pub error_sum: f32,
+    pub prev_error: f32,
 }
 
 const BIKE_BODY_COLLISION_GROUP: (Group, Group) = (Group::GROUP_1, Group::GROUP_1);
@@ -38,7 +36,7 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
     let ccd = Ccd { enabled: true };
 
     let bcollider_shape =
-        Collider::capsule(Vec3::new(0.0, 0.0, -2.7), Vec3::new(0.0, 0.0, 2.5), 1.0);
+        Collider::capsule(Vec3::new(0.0, 0.0, -1.0), Vec3::new(0.0, 0.0, 1.0), 0.7);
 
     let friction = Friction {
         coefficient: 0.0,
@@ -94,19 +92,18 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
         .insert(CyberBikeControl::default())
         .id();
 
-    // seriously the cyberbike is so fucking huge what the heck
-    let wheel_positions = vec![-5.1, 4.7, -5.1];
-    let wheel_y = -1.8f32;
+    let wheel_z_positions = vec![-1.2, 1.5, -1.2];
+    let wheel_y = -1.0f32;
 
     // re-set the collision group
     let (membership, filter) = BIKE_WHEEL_COLLISION_GROUP;
     let wheels_collision_group = CollisionGroups::new(membership, filter);
 
-    for (i, &wheel_z) in wheel_positions.iter().enumerate() {
+    for (i, &wheel_z) in wheel_z_positions.iter().enumerate() {
         let (wheel_x, wheel_rad, stiffness) = match i {
-            0 => (-2.6, 1.0, 0.016),
-            2 => (2.6, 1.0, 0.016),
-            1 => (0.0, 1.2, 0.020),
+            0 => (-1.1, 0.5, 0.026),
+            2 => (1.1, 0.5, 0.026),
+            1 => (0.0, 0.8, 0.020),
             _ => unreachable!(),
         };
         let offset = Vec3::new(wheel_x, wheel_y, wheel_z);

src/camera.rs

@@ -66,7 +66,7 @@ fn follow_cyberbike(
             CyberCameras::Debug => {
                 let pos = bike_xform.translation
                     + (bike_xform.forward() * 9.0)
-                    + (bike_xform.left() * 10.0)
+                    + (bike_xform.left() * 20.0)
                     + (bike_xform.up() * 5.0);
                 cam_xform.translation = pos;
                 cam_xform.look_at(bike_xform.translation, up);

src/main.rs

@@ -12,9 +12,9 @@ use cyber_rider::{
 };
 
 const MOVEMENT_SETTINGS: MovementSettings = MovementSettings {
-    sensitivity: 60.0, // steering
-    accel: 70.0,       // thrust
-    gravity: 7.0,
+    sensitivity: 10.0, // steering
+    accel: 20.0,       // thrust
+    gravity: 10.0,
 };
 
 fn main() {