diff --git a/src/action/components.rs b/src/action/components.rs index 8ea7c9f..1652477 100644 --- a/src/action/components.rs +++ b/src/action/components.rs @@ -15,10 +15,12 @@ impl Default for ActionDebugInstant { } impl ActionDebugInstant { + #[allow(dead_code)] pub fn reset(&mut self) { self.0 = Instant::now(); } + #[allow(dead_code)] pub fn elapsed(&self) -> Duration { self.0.elapsed() } @@ -68,9 +70,9 @@ pub struct CatControllerSettings { impl Default for CatControllerSettings { fn default() -> Self { Self { - kp: 40.0, - kd: 5.0, - ki: 0.1, + kp: 80.0, + kd: 10.0, + ki: 0.4, } } } @@ -79,11 +81,8 @@ impl Default for CatControllerSettings { 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 { @@ -91,11 +90,8 @@ impl Default for CatControllerState { Self { roll_integral: Default::default(), roll_prev: Default::default(), - pitch_integral: Default::default(), - pitch_prev: Default::default(), decay_factor: 0.99, roll_limit: 1.5, - pitch_limit: 0.1, } } } @@ -105,9 +101,6 @@ impl CatControllerState { if self.roll_integral.abs() > 0.001 { self.roll_integral *= self.decay_factor; } - if self.pitch_integral.abs() > 0.001 { - self.pitch_integral *= self.decay_factor; - } } pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) { @@ -118,16 +111,7 @@ impl CatControllerState { (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_integral_limits(&mut self, roll: f32, pitch: f32) { + pub fn set_integral_limits(&mut self, roll: f32) { self.roll_limit = roll; - self.pitch_limit = pitch; } } diff --git a/src/action/mod.rs b/src/action/mod.rs index 84a2b46..af13e47 100644 --- a/src/action/mod.rs +++ b/src/action/mod.rs @@ -1,6 +1,7 @@ use bevy::{ diagnostic::FrameTimeDiagnosticsPlugin, - prelude::{App, IntoSystemDescriptor, Plugin}, + prelude::{App, IntoSystemDescriptor, Plugin, ReflectResource, Resource}, + reflect::Reflect, }; use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin}; @@ -10,6 +11,12 @@ mod systems; pub use components::*; use systems::*; +#[derive(Resource, Default, Debug, Reflect)] +#[reflect(Resource)] +struct CyberLean { + pub lean: f32, +} + pub struct CyberActionPlugin; impl Plugin for CyberActionPlugin { fn build(&self, app: &mut App) { @@ -17,11 +24,14 @@ impl Plugin for CyberActionPlugin { .register_type::() .init_resource::() .init_resource::() + .init_resource::() + .register_type::() .register_type::() .add_plugin(RapierPhysicsPlugin::::default()) .add_plugin(FrameTimeDiagnosticsPlugin::default()) .add_system(surface_fix.label("surface_fix")) - .add_system(gravity.before("cat")) + .add_system(gravity.label("gravity").before("cat")) + .add_system(cyber_lean.before("cat").after("gravity")) .add_system(falling_cat.label("cat")) .add_system(input_forces.label("iforces").after("cat")) .add_system( diff --git a/src/action/systems.rs b/src/action/systems.rs index 4506454..f119bf5 100644 --- a/src/action/systems.rs +++ b/src/action/systems.rs @@ -1,6 +1,8 @@ -use std::f32::consts::PI; +use std::f32::consts::{FRAC_PI_3, FRAC_PI_4}; -use bevy::prelude::{Commands, Entity, Query, Res, ResMut, Time, Transform, Vec3, With, Without}; +use bevy::prelude::{ + Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without, +}; use bevy_rapier3d::prelude::{ CollisionGroups, ExternalForce, Group, MultibodyJoint, QueryFilter, RapierConfiguration, RapierContext, ReadMassProperties, Velocity, @@ -8,13 +10,26 @@ use bevy_rapier3d::prelude::{ #[cfg(feature = "inspector")] use super::ActionDebugInstant; -use super::{CatControllerSettings, CatControllerState, MovementSettings, Tunneling}; +use super::{CatControllerSettings, CatControllerState, CyberLean, MovementSettings, Tunneling}; use crate::{ bike::{CyberBikeBody, CyberSteering, CyberWheel, BIKE_WHEEL_COLLISION_GROUP}, input::InputState, }; -const PITCH_SCALE: f32 = 0.2; +fn yaw_to_angle(yaw: f32) -> f32 { + yaw.powi(3) * FRAC_PI_4 +} + +fn rotate_point(pt: &Vec3, rot: &Quat) -> Vec3 { + // thanks to https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html + let [x, y, z] = pt.to_array(); + let qpt = Quat::from_xyzw(x, y, z, 0.0); + // p' = rot^-1 * qpt * rot + let rot_qpt = rot.inverse() * qpt * *rot; + + // why does this need to be inverted??? + -Vec3::from_array([rot_qpt.x, rot_qpt.y, rot_qpt.z]) +} /// The gravity vector points from the cyberbike to the center of the planet. pub(super) fn gravity( @@ -28,21 +43,49 @@ pub(super) fn gravity( forces.torque = Vec3::ZERO; } +/// The desired lean angle, given steering input and speed. +pub(super) fn cyber_lean( + bike_state: Query<(&Velocity, &Transform), With>, + wheels: Query<&Transform, With>, + input: Res, + gravity_settings: Res, + mut lean: ResMut, +) { + let (velocity, xform) = bike_state.single(); + let vel = velocity.linvel.dot(xform.forward()); + let v_squared = vel.powi(2); + let steering_angle = yaw_to_angle(input.yaw); + let wheels: Vec<_> = wheels.iter().map(|w| w.translation).collect(); + let wheel_base = (wheels[0] - wheels[1]).length(); + let radius = wheel_base / steering_angle.tan(); + let gravity = gravity_settings.gravity; + let v2_r = v_squared / radius; + let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3); + + if tan_theta.is_finite() && !tan_theta.is_subnormal() { + lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4); + } else { + lean.lean = 0.0; + } +} + /// PID-based controller for stabilizing attitude; keeps the cyberbike upright. pub(super) fn falling_cat( mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>, time: Res