Add PID roll stabilizer.

This commit is contained in:
Joe Ardent 2022-03-14 23:58:58 -07:00
parent f26226dcb7
commit 139b8af742
2 changed files with 38 additions and 24 deletions

View file

@ -2,7 +2,7 @@ use bevy::prelude::*;
use bevy_rapier3d::{na::Vector3, prelude::*};
use crate::{
bike::{CyberBikeBody, CyberBikeCollider},
bike::{CyberBikeBody, CyberBikeCollider, CyberBikeControl},
input::InputState,
};
@ -31,28 +31,35 @@ fn gravity(
config.gravity = gravity.into();
}
fn falling_cat(
mut bike_query: Query<(&Transform, &mut RigidBodyForcesComponent), With<CyberBikeBody>>,
fn falling_cat_pid(
mut bike_query: Query<(
&Transform,
&mut RigidBodyForcesComponent,
&mut CyberBikeControl,
)>,
) {
let (bike_xform, mut forces) = bike_query.single_mut();
let up = bike_xform.translation.normalize();
let cam_up = bike_xform.up();
let cos = up.dot(cam_up);
let (xform, mut forces, mut control_vars) = bike_query.single_mut();
let up = xform.translation.normalize();
let cam_right = xform.right();
let cos = up.dot(cam_right);
let error = if cos.is_normal() { -cos } else { 0.0 };
let roll_cos = up.dot(bike_xform.right());
let derivative = error - control_vars.previous;
control_vars.previous = error;
let r_mag = if roll_cos.is_normal() {
roll_cos.powi(3) * 8.0
} else {
0.0
};
let r_tork = bike_xform.forward() * r_mag;
let integral = (control_vars.sum * 0.6) + error;
control_vars.sum = integral;
let theta = cos.acos();
let force_mag = if !theta.is_normal() { 0.0 } else { theta * 7.0 };
let torque = cam_up.cross(up).normalize() * force_mag;
let proportional = error;
forces.torque = (r_tork + torque).into();
let kp = 7.0;
let ki = 0.19;
let kd = 4.4;
let mag = (kp * proportional) + (ki * integral) + (kd * derivative);
let torque = xform.back() * mag;
forces.torque = torque.into();
}
fn drag(
@ -96,7 +103,7 @@ impl Plugin for CyberActionPlugin {
app.init_resource::<MovementSettings>()
.add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
.add_system(gravity)
.add_system(falling_cat.label("cat"))
.add_system(falling_cat_pid.label("cat"))
.add_system(drag.label("drag").after("iforces"))
.add_system(input_forces.label("iforces").after("cat"));
}

View file

@ -1,4 +1,4 @@
use bevy::prelude::*;
use bevy::{prelude::*, utils::Instant};
use bevy_rapier3d::prelude::*;
use crate::planet::PLANET_RADIUS;
@ -14,6 +14,12 @@ pub struct CyberBikeCollider;
#[derive(Component, Debug)]
pub struct CyberBikeModel;
#[derive(Component, Debug, Default, Clone, Copy)]
pub struct CyberBikeControl {
pub sum: f32,
pub previous: f32,
}
const BIKE_BODY_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01);
const BIKE_WHEEL_GROUP: InteractionGroups = InteractionGroups::new(0b10, 0b10);
@ -78,6 +84,7 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
})
.insert(CyberBikeModel)
.insert(CyberBikeBody)
.insert(CyberBikeControl::default())
.id();
// seriously the cyberbike is so fucking huge what the heck
@ -86,9 +93,9 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
for (i, &wheel_z) in wheel_positions.iter().enumerate() {
let (wheel_x, wheel_rad, stiffness) = match i {
0 => (-2.6, 0.95, 0.014),
2 => (2.6, 0.95, 0.014),
1 => (0.0, 1.2, 0.019),
0 => (-2.6, 1.0, 0.016),
2 => (2.6, 1.0, 0.016),
1 => (0.0, 1.2, 0.020),
_ => unreachable!(),
};
let offset = Vec3::new(wheel_x, wheel_y, wheel_z);
@ -127,7 +134,7 @@ fn spawn_cyberbike(mut commands: Commands, asset_server: Res<AssetServer>) {
.insert(ColliderDebugRender::from(Color::YELLOW))
.id();
let damping = 0.25;
let damping = 0.3;
let prismatic = PrismaticJoint::new(Vector::y_axis())
.local_anchor1(offset.into())