diff --git a/src/action/components.rs b/src/action/components.rs index 8ea7c9f..af6817c 100644 --- a/src/action/components.rs +++ b/src/action/components.rs @@ -68,9 +68,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, } } } diff --git a/src/action/mod.rs b/src/action/mod.rs index abb282a..af13e47 100644 --- a/src/action/mod.rs +++ b/src/action/mod.rs @@ -30,8 +30,8 @@ impl Plugin for CyberActionPlugin { .add_plugin(RapierPhysicsPlugin::::default()) .add_plugin(FrameTimeDiagnosticsPlugin::default()) .add_system(surface_fix.label("surface_fix")) - .add_system(gravity.before("cat")) - .add_system(cyber_lean.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 4a9501a..19b9ad9 100644 --- a/src/action/systems.rs +++ b/src/action/systems.rs @@ -1,4 +1,4 @@ -use std::f32::consts::PI; +use std::f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4}; use bevy::prelude::{ Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without, @@ -19,7 +19,7 @@ use crate::{ const PITCH_SCALE: f32 = 0.2; fn yaw_to_angle(yaw: f32) -> f32 { - yaw.powi(3) * (PI / 4.0) + yaw.powi(3) * FRAC_PI_4 } /// The gravity vector points from the cyberbike to the center of the planet. @@ -36,21 +36,40 @@ pub(super) fn gravity( /// The desired lean angle, given steering input and speed. pub(super) fn cyber_lean( - velocity: Query<&Velocity, With>, + mut bike_state: Query< + ( + &Velocity, + &mut ExternalForce, + &Transform, + &ReadMassProperties, + ), + With, + >, wheels: Query<&Transform, With>, input: Res, gravity_settings: Res, mut lean: ResMut, ) { - let vel_squared = velocity.single().linvel.length_squared(); + let (velocity, mut forces, xform, mass_props) = bike_state.single_mut(); + 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 tan_theta = vel_squared / (radius * gravity); + let v2_r = v_squared / radius; + let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3); - lean.lean = (PI / 2.0) - tan_theta.atan(); + let up = xform.translation.normalize(); + let right = up.cross(xform.back()).normalize(); + + let centripetal = -right * (mass_props.0.mass * v2_r); + + if v2_r.is_normal() { + forces.force += centripetal; + lean.lean = -tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4); + } } /// PID-based controller for stabilizing attitude; keeps the cyberbike upright. @@ -62,15 +81,23 @@ pub(super) fn falling_cat( #[cfg(feature = "inspector")] mut debug_instant: ResMut, ) { let (xform, mut forces, mut control_vars) = bike_query.single_mut(); - let mut wup = Transform::from_translation(xform.translation); - wup.rotate(Quat::from_axis_angle(xform.back(), lean.lean)); - let wup = wup.up(); + let wup = xform.translation.normalize(); //Transform::from_translation(xform.translation.normalize()); + let rot = Quat::from_axis_angle(xform.forward(), lean.lean); + let [x, y, z] = wup.to_array(); + let qup = Quat::from_xyzw(x, y, z, 0.0); // turn our "world up" point into a Quat + + // p' = rot^-1 * qup * rot + let wup = rot.inverse() * qup * rot; + let wup = Vec3::from_array([wup.x, wup.y, wup.z]).normalize(); let bike_up = xform.up(); - let wright = xform.forward().cross(wup).normalize(); + let wright = xform + .forward() + .cross(xform.translation.normalize()) + .normalize(); - let roll_error = wright.dot(bike_up); - let pitch_error = wup.dot(xform.back()); + let roll_error = wright.dot(wup); + let pitch_error = xform.translation.normalize().dot(xform.forward()); // only try to correct roll if we're not totally vertical if pitch_error.abs() < 0.95 { @@ -80,6 +107,13 @@ pub(super) fn falling_cat( if mag.is_finite() { forces.torque += xform.back() * mag; } + #[cfg(feature = "inspector")] + { + if debug_instant.elapsed().as_millis() > 5000 { + dbg!(&control_vars, mag, &wup); + debug_instant.reset(); + } + } } // we can do pitch correction any time, it's not coupled to roll @@ -89,17 +123,10 @@ pub(super) fn falling_cat( let mag = (settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative); if mag.is_finite() { - forces.torque += wright * mag; + //forces.torque += wright * mag; } - #[cfg(feature = "inspector")] - { - if debug_instant.elapsed().as_millis() > 1000 { - dbg!(&control_vars, mag, &lean); - debug_instant.reset(); - } - } - control_vars.decay(); + //control_vars.decay(); } /// Apply forces to the cyberbike as a result of input.