From b9d49f6e1c6d07e9a2355001849c03628ab679b8 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Sun, 29 Jan 2023 14:53:47 -0800 Subject: [PATCH] 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); + }); } }