Add PID roll stabilizer.
This commit is contained in:
parent
f26226dcb7
commit
139b8af742
2 changed files with 38 additions and 24 deletions
|
@ -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"));
|
||||
}
|
||||
|
|
17
src/bike.rs
17
src/bike.rs
|
@ -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())
|
||||
|
|
Loading…
Reference in a new issue