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

use real PID for stability control

Browse source
This commit is contained in:
Joe Ardent 2023-01-29 14:53:47 -08:00
parent 99d58cbcac
commit b9d49f6e1c
2 changed files with 78 additions and 40 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

92
src/action.rs
View file

@ -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<RapierConfiguration>) {
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<CatControllerSettings>,
) {
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<MovementSettings>,
input: Res<InputState>,
mut cquery: Query<&mut Friction, With<CyberBikeCollider>>,
mut cquery: Query<&mut Friction, With<CyberWheel>>,
mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
) {
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;
}

26
src/bike.rs
View file

@ -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<Mesh>>,
@ -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);
});
}
}