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

merge cyber-lean

Browse source
This commit is contained in:
Joe Ardent 2023-02-21 14:28:28 -08:00
commit 630594ee97
3 changed files with 79 additions and 54 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

28
src/action/components.rs
View file

@ -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;
}
}

14
src/action/mod.rs
View file

@ -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::<MovementSettings>()
.init_resource::<CatControllerSettings>()
.init_resource::<ActionDebugInstant>()
.init_resource::<CyberLean>()
.register_type::<CyberLean>()
.register_type::<CatControllerSettings>()
.add_plugin(RapierPhysicsPlugin::<NoUserData>::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(

83
src/action/systems.rs
View file

@ -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<CyberBikeBody>>,
wheels: Query<&Transform, With<CyberWheel>>,
input: Res<InputState>,
gravity_settings: Res<MovementSettings>,
mut lean: ResMut<CyberLean>,
) {
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<Time>,
settings: Res<CatControllerSettings>,
lean: Res<CyberLean>,
#[cfg(feature = "inspector")] mut debug_instant: ResMut<ActionDebugInstant>,
) {
let (xform, mut forces, mut control_vars) = bike_query.single_mut();
let wup = xform.translation.normalize();
let bike_up = xform.up();
let world_up = xform.translation.normalize();
let rot = Quat::from_axis_angle(xform.back(), lean.lean);
let target_up = rotate_point(&world_up, &rot).normalize();
let wright = xform.forward().cross(wup).normalize();
let bike_right = xform.right();
let roll_error = wright.dot(bike_up);
let pitch_error = wup.dot(xform.back());
let roll_error = bike_right.dot(target_up);
let pitch_error = world_up.dot(xform.back());
// only try to correct roll if we're not totally vertical
if pitch_error.abs() < 0.95 {
@ -52,26 +95,14 @@ pub(super) fn falling_cat(
if mag.is_finite() {
forces.torque += xform.back() * mag;
}
}
// we can do pitch correction any time, it's not coupled to roll
let scaled_pitch_error = pitch_error * PITCH_SCALE;
let (derivative, integral) =
control_vars.update_pitch(scaled_pitch_error, time.delta_seconds());
let mag =
(settings.kp * scaled_pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
if mag.is_finite() {
forces.torque += wright * mag;
}
#[cfg(feature = "inspector")]
{
if debug_instant.elapsed().as_millis() > 1000 {
dbg!(&control_vars, mag);
dbg!(&control_vars, mag, &target_up);
debug_instant.reset();
}
}
control_vars.decay();
}
}
/// Apply forces to the cyberbike as a result of input.
@ -89,13 +120,13 @@ pub(super) fn input_forces(
// thrust
let thrust = xform.forward() * input.throttle * settings.accel;
let point = xform.translation + xform.back() * 0.5;
let point = xform.translation + xform.back();
let force = ExternalForce::at_point(thrust, point, xform.translation);
*forces += force;
// brake
for mut motor in braking_query.iter_mut() {
let factor = if input.brake { 200.00 } else { 0.0 };
let factor = if input.brake { 500.00 } else { 0.0 };
motor.data = (*motor
.data
.as_revolute_mut()
@ -106,7 +137,7 @@ pub(super) fn input_forces(
}
// steering
let angle = input.yaw.powf(3.0) * (PI / 4.0);
let angle = yaw_to_angle(input.yaw);
let mut steering = steering_query.single_mut();
steering.data = (*steering
.data