From 2436e27290bb57fa3bfb712a2750ceaef43fbe18 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Thu, 26 Jan 2023 22:28:28 -0800 Subject: [PATCH 01/16] try interpolating more transforms, no dice --- src/bike.rs | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/bike.rs b/src/bike.rs index 25df0d3..d785bf5 100644 --- a/src/bike.rs +++ b/src/bike.rs @@ -76,7 +76,7 @@ fn spawn_cyberbike( let mut xform = Transform::from_translation(Vec3::X * altitude) .with_rotation(Quat::from_axis_angle(Vec3::Z, -89.0f32.to_radians())); - //.with_rotation(Quat::from_axis_angle(Vec3::X, -90.0f32.to_radians())); + //.with_rotation(Quat::from_axis_angle(Vec3::X, 140.0f32.to_radians())); let right = xform.right() * 350.0; xform.translation += right; @@ -253,7 +253,7 @@ fn spawn_tires( }; let tire_spundle = SpatialBundle { - transform: wheel_pos_in_world, + transform: Transform::IDENTITY, ..Default::default() }; @@ -270,7 +270,16 @@ fn spawn_tires( wheels_collision_group, )) .with_children(|wheel| { - wheel.spawn(tire_spundle).insert(pbr_bundle.clone()); + wheel.spawn(tire_spundle).insert(pbr_bundle.clone()).insert( + TransformInterpolation { + start: None, + end: None, + }, + ); + }) + .insert(TransformInterpolation { + start: None, + end: None, }) .insert(CyberWheel); } From e524decd385211169afb7b98b71aa77bd64ce344 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 28 Jan 2023 16:46:24 -0800 Subject: [PATCH 02/16] got two wheels working pretty good next step is to make the wheels have friction and to rotate --- Cargo.lock | 90 +++++++++++++++++++++++++++------------------------ Cargo.toml | 2 +- src/action.rs | 16 +++++---- src/bike.rs | 40 +++++++++++------------ src/main.rs | 19 ++--------- 5 files changed, 81 insertions(+), 86 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index 8cedfe4..c69a894 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -37,9 +37,9 @@ dependencies = [ [[package]] name = "ahash" -version = "0.8.2" +version = "0.8.3" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "bf6ccdb167abbf410dcb915cabd428929d7f6a04980b54a11f26a39f1c7f7107" +checksum = "2c99f64d1e06488f620f932677e24bc6e2897582980441ae90a671415bd7ec2f" dependencies = [ "cfg-if", "once_cell", @@ -949,9 +949,9 @@ checksum = "c1db59621ec70f09c5e9b597b220c7a2b43611f4710dc03ceb8748637775692c" [[package]] name = "cc" -version = "1.0.78" +version = "1.0.79" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a20104e2335ce8a659d6dd92a51a767a0c062599c73b343fd152cb401e828c3d" +checksum = "50d30906286121d95be3d479533b458f87493b30a4b5f79a607db8f5d11aa91f" [[package]] name = "cesu8" @@ -1303,16 +1303,16 @@ version = "0.20.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "65a5e883a316e53866977450eecfbcac9c48109c2ab3394af29feb83fcde4ea9" dependencies = [ - "ahash 0.8.2", + "ahash 0.8.3", "epaint", "nohash-hasher", ] [[package]] name = "either" -version = "1.8.0" +version = "1.8.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "90e5c1c8368803113bf0c9584fc495a58b86dc8a29edbf8fe877d21d9507e797" +checksum = "7fcaabb2fef8c910e7f4c7ce9f67a1283a1715879a7c230ca9d6d1ae31f16d91" [[package]] name = "emath" @@ -1372,7 +1372,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "de14b65fe5e423e0058f77a8beb2c863b056d0566d6c4ce0d097aa5814cb705a" dependencies = [ "ab_glyph", - "ahash 0.8.2", + "ahash 0.8.3", "atomic_refcell", "bytemuck", "ecolor", @@ -1891,12 +1891,6 @@ dependencies = [ "winapi", ] -[[package]] -name = "libm" -version = "0.1.4" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7fc7aa29613bd6a620df431842069224d8bc9011086b1db4c0e0cd47fa03ec9a" - [[package]] name = "libm" version = "0.2.6" @@ -2176,6 +2170,15 @@ dependencies = [ "rand_xorshift", ] +[[package]] +name = "nom8" +version = "0.2.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ae01545c9c7fc4486ab7debaf2aad7003ac19431791868fb2e8066df97fad2f8" +dependencies = [ + "memchr", +] + [[package]] name = "nu-ansi-term" version = "0.46.0" @@ -2234,23 +2237,23 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "578ede34cf02f8924ab9447f50c28075b4d3e5b269972345e7e0372b38c6cdcd" dependencies = [ "autocfg", - "libm 0.2.6", + "libm", ] [[package]] name = "num_enum" -version = "0.5.7" +version = "0.5.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cf5395665662ef45796a4ff5486c5d41d29e0c09640af4c5f17fd94ee2c119c9" +checksum = "8d829733185c1ca374f17e52b762f24f535ec625d2cc1f070e34c8a9068f341b" dependencies = [ "num_enum_derive", ] [[package]] name = "num_enum_derive" -version = "0.5.7" +version = "0.5.9" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3b0498641e53dd6ac1a4f22547548caa6864cc4933784319cd1775271c5a46ce" +checksum = "2be1598bf1c313dcdd12092e3f1920f463462525a21b7b4e11b4168353d0123e" dependencies = [ "proc-macro-crate", "proc-macro2", @@ -2324,16 +2327,6 @@ dependencies = [ "ttf-parser", ] -[[package]] -name = "packed_simd_2" -version = "0.3.8" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a1914cd452d8fccd6f9db48147b29fd4ae05bea9dc5d9ad578509f72415de282" -dependencies = [ - "cfg-if", - "libm 0.1.4", -] - [[package]] name = "parking" version = "2.0.0" @@ -2447,19 +2440,18 @@ checksum = "5b40af805b3121feab8a3c29f04d8ad262fa8e0561883e7653e024ae4479e6de" [[package]] name = "pretty-type-name" -version = "1.0.0" +version = "1.0.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a8815d101cfb4cb491154896bdab292a395a7ac9ab185a9941a2f5be0135900d" +checksum = "f0f73cdaf19b52e6143685c3606206e114a4dfa969d6b14ec3894c88eb38bd4b" [[package]] name = "proc-macro-crate" -version = "1.2.1" +version = "1.3.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "eda0fc3b0fb7c975631757e14d9049da17374063edb6ebbcbc54d880d4fe94e9" +checksum = "66618389e4ec1c7afe67d51a9bf34ff9236480f8d51e7489b7d5ab0303c13f34" dependencies = [ "once_cell", - "thiserror", - "toml", + "toml_edit", ] [[package]] @@ -2539,9 +2531,9 @@ checksum = "63e935c45e09cc6dcf00d2f0b2d630a58f4095320223d47fc68918722f0538b6" [[package]] name = "rapier3d" -version = "0.17.0" +version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8aa1871dbbf25b2d91832c5f426827defc648d5340607870dc05471ab2748522" +checksum = "33c3a4adb69feadac44d74be058aaaec4502f9c193ec20c0e629b07387ee61a2" dependencies = [ "approx", "arrayvec", @@ -2555,7 +2547,6 @@ dependencies = [ "parry3d", "rustc-hash", "simba", - "vec_map", ] [[package]] @@ -2748,7 +2739,6 @@ dependencies = [ "approx", "num-complex", "num-traits", - "packed_simd_2", "paste", "wide", ] @@ -2923,6 +2913,23 @@ dependencies = [ "serde", ] +[[package]] +name = "toml_datetime" +version = "0.5.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4553f467ac8e3d374bc9a177a26801e5d0f9b211aa1673fb137a403afd1c9cf5" + +[[package]] +name = "toml_edit" +version = "0.18.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "56c59d8dd7d0dcbc6428bf7aa2f0e823e26e43b3c9aca15bbc9475d23e5fa12b" +dependencies = [ + "indexmap", + "nom8", + "toml_datetime", +] + [[package]] name = "tracing" version = "0.1.37" @@ -3181,9 +3188,9 @@ dependencies = [ [[package]] name = "webbrowser" -version = "0.8.4" +version = "0.8.6" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e74f5ff7786c4c21f61ba8e30ea29c9745f06fca0a4a02d083b3c662583399e8" +checksum = "769f1a8831de12cad7bd6f9693b15b1432d93a151557810f617f626af823acae" dependencies = [ "core-foundation", "dirs", @@ -3194,7 +3201,6 @@ dependencies = [ "raw-window-handle 0.5.0", "url", "web-sys", - "windows", ] [[package]] diff --git a/Cargo.toml b/Cargo.toml index e8c7651..21e1997 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -28,7 +28,7 @@ features = [ ] [dependencies.bevy_rapier3d] -features = ["simd-nightly", "debug-render-3d"] +features = ["debug-render-3d"] version = "0.20" # Maybe also enable only a small amount of optimization for our code: diff --git a/src/action.rs b/src/action.rs index 31c2c0b..48cf3a1 100644 --- a/src/action.rs +++ b/src/action.rs @@ -11,7 +11,8 @@ use crate::{ input::InputState, }; -#[derive(Resource)] +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] pub struct MovementSettings { pub accel: f32, pub gravity: f32, @@ -21,9 +22,9 @@ pub struct MovementSettings { impl Default for MovementSettings { fn default() -> Self { Self { - sensitivity: 10.0, - accel: 20.0, - gravity: 9.8, + sensitivity: 6.0, + accel: 40.0, + gravity: 5.0, } } } @@ -113,8 +114,10 @@ fn input_forces( friction.coefficient = if input.brake { 2.0 } else { 0.0 }; // steering - let torque = xform.down() * input.yaw * settings.sensitivity; - forces.torque += torque; + let force = xform.right() * input.yaw * settings.sensitivity; + let point = xform.translation + xform.forward() + Vec3::new(0.5, 0.0, 0.0); + let force = ExternalForce::at_point(force, point, xform.translation); + *forces += force; } fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With>) { @@ -130,6 +133,7 @@ pub struct CyberActionPlugin; impl Plugin for CyberActionPlugin { fn build(&self, app: &mut App) { app.init_resource::() + .register_type::() .init_resource::() .register_type::() .add_plugin(RapierPhysicsPlugin::::default()) diff --git a/src/bike.rs b/src/bike.rs index d785bf5..7068ee6 100644 --- a/src/bike.rs +++ b/src/bike.rs @@ -5,7 +5,7 @@ use bevy_rapier3d::{ geometry::Group, prelude::{ Ccd, Collider, ColliderMassProperties, CollisionGroups, Damping, ExternalForce, Friction, - ImpulseJoint, PrismaticJointBuilder, Restitution, RigidBody, Sleeping, + MultibodyJoint, PrismaticJointBuilder, Restitution, RigidBody, Sleeping, TransformInterpolation, Velocity, }, }; @@ -54,11 +54,11 @@ impl Default for WheelConfig { front_forward: 0.9, front_stance: 0.65, rear_back: 1.1, - y: -1.5, - limits: [-1.0, 0.0], - stiffness: 80.0, - damping: 0.6, - radius: 0.4, + y: -0.4, + limits: [-0.5, 0.1], + stiffness: 75.0, + damping: 2.0, + radius: 0.3, } } } @@ -147,6 +147,7 @@ fn spawn_cyberbike( spawn_tires(&mut commands, &xform, bike, &wheel_conf, &mut meshterials); } +#[cfg(feature = "inspector")] fn re_tire( mut commands: Commands, wheel_conf: ResMut, @@ -171,6 +172,7 @@ fn spawn_tires( conf: &WheelConfig, meshterials: &mut Meshterial, ) { + //return; // re-set the collision group let (membership, filter) = BIKE_WHEEL_COLLISION_GROUP; let wheels_collision_group = CollisionGroups::new(membership, filter); @@ -204,19 +206,11 @@ fn spawn_tires( ..Default::default() }; - let mut wheel_poses = Vec::with_capacity(3); + let mut wheel_poses = Vec::with_capacity(2); - // left front + // front { - let wheel_x = -conf.front_stance; - let wheel_z = -conf.front_forward; - let offset = Vec3::new(wheel_x, wheel_y, wheel_z); - wheel_poses.push(offset); - } - - // right front - { - let wheel_x = conf.front_stance; + let wheel_x = 0.0; let wheel_z = -conf.front_forward; let offset = Vec3::new(wheel_x, wheel_y, wheel_z); wheel_poses.push(offset); @@ -244,8 +238,8 @@ fn spawn_tires( let prismatic = PrismaticJointBuilder::new(Vec3::Y) .local_anchor1(offset) .limits(limits) - .motor_position(-1.0, stiffness, damping); - let joint = ImpulseJoint::new(bike, prismatic); + .motor_position(-0.1, stiffness, damping); + let joint = MultibodyJoint::new(bike, prismatic); let spatial_bundle = SpatialBundle { transform: wheel_pos_in_world, @@ -290,7 +284,11 @@ impl Plugin for CyberBikePlugin { fn build(&self, app: &mut App) { app.insert_resource(WheelConfig::default()) .register_type::() - .add_startup_system_to_stage(StartupStage::PostStartup, spawn_cyberbike) - .add_system(re_tire); + .add_startup_system_to_stage(StartupStage::PostStartup, spawn_cyberbike); + + #[cfg(feature = "inspector")] + { + app.add_system(re_tire); + } } } diff --git a/src/main.rs b/src/main.rs index 8ee6143..4c203ab 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,20 +1,8 @@ use bevy::prelude::*; use cyber_rider::{ - action::{CyberActionPlugin, MovementSettings}, - bike::CyberBikePlugin, - camera::CyberCamPlugin, - disable_mouse_trap, - glamor::CyberGlamorPlugin, - input::CyberInputPlugin, - lights::CyberSpaceLightsPlugin, - planet::CyberPlanetPlugin, - ui::CyberUIPlugin, -}; - -const MOVEMENT_SETTINGS: MovementSettings = MovementSettings { - sensitivity: 8.0, // steering - accel: 30.0, // thrust - gravity: 9.0, + action::CyberActionPlugin, bike::CyberBikePlugin, camera::CyberCamPlugin, disable_mouse_trap, + glamor::CyberGlamorPlugin, input::CyberInputPlugin, lights::CyberSpaceLightsPlugin, + planet::CyberPlanetPlugin, ui::CyberUIPlugin, }; fn main() { @@ -29,7 +17,6 @@ fn main() { }, ..Default::default() })) - .insert_resource(MOVEMENT_SETTINGS) .add_plugin(CyberPlanetPlugin) .add_plugin(CyberGlamorPlugin) .add_plugin(CyberInputPlugin) From c04fb9fa119975c185e683aa4f907ebd5894f892 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 28 Jan 2023 16:57:44 -0800 Subject: [PATCH 03/16] remove 'CyberBikeModel' marker component --- src/bike.rs | 4 ---- src/camera.rs | 4 ++-- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/bike.rs b/src/bike.rs index 7068ee6..c8fa67f 100644 --- a/src/bike.rs +++ b/src/bike.rs @@ -23,9 +23,6 @@ pub struct CyberBikeBody; #[derive(Component)] pub struct CyberBikeCollider; -#[derive(Component, Debug)] -pub struct CyberBikeModel; - #[derive(Debug, Component)] pub struct CyberWheel; @@ -139,7 +136,6 @@ fn spawn_cyberbike( ..Default::default() }); }) - .insert(CyberBikeModel) .insert(CyberBikeBody) .insert(CyberBikeControl::default()) .id(); diff --git a/src/camera.rs b/src/camera.rs index 3ecd5e6..ef47adc 100644 --- a/src/camera.rs +++ b/src/camera.rs @@ -1,6 +1,6 @@ use bevy::prelude::*; -use crate::{bike::CyberBikeModel, input::InputState}; +use crate::{bike::CyberBikeBody, input::InputState}; // 85 degrees in radians const MAX_PITCH: f32 = 1.48353; @@ -57,7 +57,7 @@ fn setup_cybercams(mut commands: Commands) { fn follow_cyberbike( mut query: ParamSet<( // 0: the bike - Query<&Transform, With>, + Query<&Transform, With>, // 1: the cameras Query<(&mut Transform, &CyberCameras)>, )>, From 99d58cbcac282c3637a66aa0cb03394dc14ae3d8 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sat, 28 Jan 2023 22:10:21 -0800 Subject: [PATCH 04/16] checkpoint --- src/action.rs | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/action.rs b/src/action.rs index 48cf3a1..f3d338c 100644 --- a/src/action.rs +++ b/src/action.rs @@ -69,6 +69,9 @@ fn falling_cat( let up = xform.translation.normalize(); let bike_up = xform.up(); + let wright = xform.right().cross(up); + let wback = wright.cross(up); + let torque = bike_up.cross(up).normalize_or_zero(); let cos = up.dot(bike_up); let cos = if cos.is_finite() { cos } else { 1.0 }; From b9d49f6e1c6d07e9a2355001849c03628ab679b8 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 29 Jan 2023 14:53:47 -0800 Subject: [PATCH 05/16] use real PID for stability control --- src/action.rs | 92 +++++++++++++++++++++++++++++++++++++-------------- src/bike.rs | 26 ++++++--------- 2 files changed, 78 insertions(+), 40 deletions(-) diff --git a/src/action.rs b/src/action.rs index f3d338c..eba0631 100644 --- a/src/action.rs +++ b/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) { 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, ) { 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 wback = wright.cross(up); + let wright = xform.forward().cross(wup).normalize(); + //let wback = wright.cross(wup); - let torque = bike_up.cross(up).normalize_or_zero(); - let cos = up.dot(bike_up); - let cos = if cos.is_finite() { cos } else { 1.0 }; + let roll_error = wright.dot(bike_up); + let pitch_error = wup.dot(xform.back()); - 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; - control_vars.prev_error = error; - // this integral term is not an integral, it's more like a weighted moving sum - let weighted_sum = control_vars.error_sum + error; - control_vars.error_sum = weighted_sum * 0.8; + let mag = + (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative); + forces.torque += xform.back() * mag; + }; - 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); } } - - forces.torque = torque * mag; + control_vars.decay(); } fn input_forces( settings: Res, input: Res, - mut cquery: Query<&mut Friction, With>, + mut cquery: Query<&mut Friction, With>, mut bquery: Query<(&Transform, &mut ExternalForce), With>, ) { 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 - friction.coefficient = if input.brake { 2.0 } else { 0.0 }; + 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; } diff --git a/src/bike.rs b/src/bike.rs index c8fa67f..14da732 100644 --- a/src/bike.rs +++ b/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>, @@ -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); + }); } } From 79b2d4b1754995c7bb1a0c7caa36048a98597e6b Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 29 Jan 2023 17:02:16 -0800 Subject: [PATCH 06/16] break up action module, actually do real for real PID --- src/action.rs | 192 --------------------------------------- src/action/components.rs | 108 ++++++++++++++++++++++ src/action/mod.rs | 29 ++++++ src/action/systems.rs | 109 ++++++++++++++++++++++ src/bike.rs | 4 +- 5 files changed, 248 insertions(+), 194 deletions(-) delete mode 100644 src/action.rs create mode 100644 src/action/components.rs create mode 100644 src/action/mod.rs create mode 100644 src/action/systems.rs diff --git a/src/action.rs b/src/action.rs deleted file mode 100644 index eba0631..0000000 --- a/src/action.rs +++ /dev/null @@ -1,192 +0,0 @@ -use bevy::{ - diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin}, - prelude::*, -}; -use bevy_rapier3d::prelude::{ - ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity, -}; - -use crate::{ - bike::{CyberBikeBody, CyberWheel}, - input::InputState, -}; - -#[derive(Debug, Resource, Reflect)] -#[reflect(Resource)] -pub struct MovementSettings { - pub accel: f32, - pub gravity: f32, - pub sensitivity: f32, -} - -impl Default for MovementSettings { - fn default() -> Self { - Self { - sensitivity: 6.0, - accel: 40.0, - gravity: 7.0, - } - } -} - -#[derive(Debug, Resource, Reflect)] -#[reflect(Resource)] -struct CatControllerSettings { - pub kp: f32, - pub kd: f32, - pub ki: f32, -} - -impl Default for CatControllerSettings { - fn default() -> Self { - Self { - kp: 17.0, - kd: 4.0, - 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) { - config.gravity = Vec3::ZERO; -} - -fn gravity( - mut query: Query<(&Transform, &mut ExternalForce), With>, - settings: Res, -) { - let (xform, mut forces) = query.single_mut(); - let grav = xform.translation.normalize() * -settings.gravity; - forces.force = grav; - forces.torque = Vec3::ZERO; -} - -fn falling_cat( - mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>, - _diagnostics: Res, - settings: Res, -) { - let (xform, mut forces, mut control_vars) = bike_query.single_mut(); - let wup = xform.translation.normalize(); - let bike_up = xform.up(); - - let wright = xform.forward().cross(wup).normalize(); - //let wback = wright.cross(wup); - - let roll_error = wright.dot(bike_up); - let pitch_error = wup.dot(xform.back()); - - // 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 mag = - (settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative); - forces.torque += xform.back() * mag; - }; - - // 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 - .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT) - .and_then(|d| d.smoothed()) - .map(|x| x as u64) - { - if count % 30 == 0 { - dbg!(roll_error, pitch_error, &control_vars); - } - } - control_vars.decay(); -} - -fn input_forces( - settings: Res, - input: Res, - mut cquery: Query<&mut Friction, With>, - mut bquery: Query<(&Transform, &mut ExternalForce), With>, -) { - let (xform, mut forces) = bquery.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(); - let force = ExternalForce::at_point(force, point, xform.translation); - *forces += force; -} - -fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With>) { - let (vels, mut forces) = query.single_mut(); - - if let Some(vel) = vels.linvel.try_normalize() { - let v2 = vels.linvel.length_squared(); - forces.force -= vel * (v2 * 0.02); - } -} - -pub struct CyberActionPlugin; -impl Plugin for CyberActionPlugin { - fn build(&self, app: &mut App) { - app.init_resource::() - .register_type::() - .init_resource::() - .register_type::() - .add_plugin(RapierPhysicsPlugin::::default()) - .add_plugin(FrameTimeDiagnosticsPlugin::default()) - .add_startup_system(zero_gravity) - .add_system(gravity.before("cat")) - .add_system(falling_cat.label("cat")) - .add_system(input_forces.label("iforces").after("cat")) - .add_system(drag.label("drag").after("iforces")); - } -} diff --git a/src/action/components.rs b/src/action/components.rs new file mode 100644 index 0000000..f16fcf9 --- /dev/null +++ b/src/action/components.rs @@ -0,0 +1,108 @@ +use std::time::Instant; + +use bevy::{ + prelude::{Component, ReflectResource, Resource}, + reflect::Reflect, +}; + +#[derive(Debug, Resource)] +pub(crate) struct ActionDebugInstant(pub Instant); + +impl Default for ActionDebugInstant { + fn default() -> Self { + ActionDebugInstant(Instant::now()) + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct MovementSettings { + pub accel: f32, + pub gravity: f32, + pub sensitivity: f32, +} + +impl Default for MovementSettings { + fn default() -> Self { + Self { + sensitivity: 6.0, + accel: 40.0, + gravity: 7.0, + } + } +} + +#[derive(Debug, Resource, Reflect)] +#[reflect(Resource)] +pub struct CatControllerSettings { + pub kp: f32, + pub kd: f32, + pub ki: f32, +} + +impl Default for CatControllerSettings { + fn default() -> Self { + Self { + kp: 17.0, + kd: 4.0, + ki: 0.05, + } + } +} + +#[derive(Component, Debug, Clone, Copy)] +pub struct CatControllerState { + pub roll_integral: f32, + pub roll_prev: f32, + pub pitch_integral: f32, + pub pitch_prev: f32, + decay_factor: f32, + roll_limit: f32, + pitch_limit: f32, +} + +impl Default for CatControllerState { + fn default() -> Self { + Self { + roll_integral: Default::default(), + roll_prev: Default::default(), + pitch_integral: Default::default(), + pitch_prev: Default::default(), + decay_factor: 0.9, + roll_limit: 1.5, + pitch_limit: 1.2, + } + } +} + +impl CatControllerState { + pub fn decay(&mut self) { + if self.roll_integral.abs() > 0.1 { + self.roll_integral *= self.decay_factor; + } + if self.pitch_integral.abs() > 0.1 { + self.pitch_integral *= self.decay_factor; + } + } + + pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) { + let lim = self.roll_limit; + self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim); + let derivative = (error - self.roll_prev) / dt; + self.roll_prev = error; + (derivative, self.roll_integral) + } + + pub fn update_pitch(&mut self, error: f32, dt: f32) -> (f32, f32) { + let lim = self.pitch_limit; + self.pitch_integral = (self.pitch_integral + (error * dt)).min(lim).max(-lim); + let derivative = (error - self.pitch_prev) / dt; + self.pitch_prev = error; + (derivative, self.pitch_integral) + } + + pub fn set_limits(&mut self, roll: f32, pitch: f32) { + self.roll_limit = roll; + self.pitch_limit = pitch; + } +} diff --git a/src/action/mod.rs b/src/action/mod.rs new file mode 100644 index 0000000..8646ba7 --- /dev/null +++ b/src/action/mod.rs @@ -0,0 +1,29 @@ +use bevy::{ + diagnostic::FrameTimeDiagnosticsPlugin, + prelude::{App, IntoSystemDescriptor, Plugin}, +}; +use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin}; + +mod components; +mod systems; + +pub use components::*; +use systems::*; + +pub struct CyberActionPlugin; +impl Plugin for CyberActionPlugin { + fn build(&self, app: &mut App) { + app.init_resource::() + .register_type::() + .init_resource::() + .init_resource::() + .register_type::() + .add_plugin(RapierPhysicsPlugin::::default()) + .add_plugin(FrameTimeDiagnosticsPlugin::default()) + .add_startup_system(zero_gravity) + .add_system(gravity.before("cat")) + .add_system(falling_cat.label("cat")) + .add_system(input_forces.label("iforces").after("cat")) + .add_system(drag.label("drag").after("iforces")); + } +} diff --git a/src/action/systems.rs b/src/action/systems.rs new file mode 100644 index 0000000..29b5d8b --- /dev/null +++ b/src/action/systems.rs @@ -0,0 +1,109 @@ +#[cfg(feature = "inspector")] +use std::time::Instant; + +use bevy::prelude::{Query, Res, ResMut, Time, Transform, Vec3, With}; +use bevy_rapier3d::prelude::{ExternalForce, Friction, RapierConfiguration, Velocity}; + +#[cfg(feature = "inspector")] +use super::ActionDebugInstant; +use super::{CatControllerSettings, CatControllerState, MovementSettings}; +use crate::{ + bike::{CyberBikeBody, CyberWheel}, + input::InputState, +}; + +/// Disable gravity in Rapier. +pub(crate) fn zero_gravity(mut config: ResMut) { + config.gravity = Vec3::ZERO; +} + +/// The gravity vector points from the cyberbike to the center of the planet. +pub(crate) fn gravity( + mut query: Query<(&Transform, &mut ExternalForce), With>, + settings: Res, +) { + let (xform, mut forces) = query.single_mut(); + let grav = xform.translation.normalize() * -settings.gravity; + forces.force = grav; + forces.torque = Vec3::ZERO; +} + +/// PID-based controller for stabilizing attitude; keeps the cyberbike upright. +pub(crate) fn falling_cat( + mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>, + time: Res