From f65282bb080589cdbcaeb93cedf5b94c946d2042 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 21 Jan 2023 19:51:01 -0800 Subject: [PATCH] fix PID code, fuckin' with shit still. --- src/action.rs | 110 +++++++++++++++++++++----------------------------- src/bike.rs | 21 +++++----- src/camera.rs | 2 +- src/main.rs | 6 +-- 4 files changed, 60 insertions(+), 79 deletions(-) diff --git a/src/action.rs b/src/action.rs index 3edf6d2..893c6d7 100644 --- a/src/action.rs +++ b/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, ) { - 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>) 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::() .add_plugin(RapierPhysicsPlugin::::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")) diff --git a/src/bike.rs b/src/bike.rs index 46c998f..4abc585 100644 --- a/src/bike.rs +++ b/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) { 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) { .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); diff --git a/src/camera.rs b/src/camera.rs index d2c2822..4ed58a5 100644 --- a/src/camera.rs +++ b/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); diff --git a/src/main.rs b/src/main.rs index 250d20e..b0936f5 100644 --- a/src/main.rs +++ b/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() {