nebkor/cyber_rider
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

re-enable pitch stabilization

Browse source
This commit is contained in:
Joe Ardent 2023-02-21 13:25:55 -08:00
parent 7fe4fbe423
commit e9fed1940e
1 changed files with 6 additions and 22 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

28
src/action/systems.rs
View file

@ -1,4 +1,4 @@
use std::f32::consts::{FRAC_PI_2, FRAC_PI_3, FRAC_PI_4}; use std::f32::consts::{FRAC_PI_3, FRAC_PI_4};
use bevy::prelude::{ use bevy::prelude::{
Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without, Commands, Entity, Quat, Query, Res, ResMut, Time, Transform, Vec3, With, Without,
@ -36,21 +36,13 @@ pub(super) fn gravity(
/// The desired lean angle, given steering input and speed. /// The desired lean angle, given steering input and speed.
pub(super) fn cyber_lean( pub(super) fn cyber_lean(
mut bike_state: Query< bike_state: Query<(&Velocity, &Transform), With<CyberBikeBody>>,
(
&Velocity,
&mut ExternalForce,
&Transform,
&ReadMassProperties,
),
With<CyberBikeBody>,
>,
wheels: Query<&Transform, With<CyberWheel>>, wheels: Query<&Transform, With<CyberWheel>>,
input: Res<InputState>, input: Res<InputState>,
gravity_settings: Res<MovementSettings>, gravity_settings: Res<MovementSettings>,
mut lean: ResMut<CyberLean>, mut lean: ResMut<CyberLean>,
) { ) {
let (velocity, mut forces, xform, mass_props) = bike_state.single_mut(); let (velocity, xform) = bike_state.single();
let vel = velocity.linvel.dot(xform.forward()); let vel = velocity.linvel.dot(xform.forward());
let v_squared = vel.powi(2); let v_squared = vel.powi(2);
let steering_angle = yaw_to_angle(input.yaw); let steering_angle = yaw_to_angle(input.yaw);
@ -61,13 +53,7 @@ pub(super) fn cyber_lean(
let v2_r = v_squared / radius; let v2_r = v_squared / radius;
let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3); let tan_theta = (v2_r / gravity).clamp(-FRAC_PI_3, FRAC_PI_3);
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() { if v2_r.is_normal() {
//forces.force += centripetal;
lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4); lean.lean = tan_theta.atan().clamp(-FRAC_PI_4, FRAC_PI_4);
} }
} }
@ -91,10 +77,10 @@ pub(super) fn falling_cat(
let target_up = -Vec3::from_array([rot_qup.x, rot_qup.y, rot_qup.z]).normalize(); let target_up = -Vec3::from_array([rot_qup.x, rot_qup.y, rot_qup.z]).normalize();
let bike_right = xform.right(); let bike_right = xform.right();
//let world_right = xform.forward().cross(world_up).normalize(); let world_right = xform.forward().cross(world_up).normalize();
let roll_error = bike_right.dot(target_up); let roll_error = bike_right.dot(target_up);
let pitch_error = world_up.dot(xform.forward()); let pitch_error = world_up.dot(xform.back());
// only try to correct roll if we're not totally vertical // only try to correct roll if we're not totally vertical
if pitch_error.abs() < 0.95 { if pitch_error.abs() < 0.95 {
@ -120,10 +106,8 @@ pub(super) fn falling_cat(
let mag = let mag =
(settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative); (settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
if mag.is_finite() { if mag.is_finite() {
//forces.torque += wright * mag; forces.torque += world_right * mag;
} }
//control_vars.decay();
} }
/// Apply forces to the cyberbike as a result of input. /// Apply forces to the cyberbike as a result of input.