break up action module, actually do real for real PID
This commit is contained in:
Joe Ardent
parent
b9d49f6e1c
commit
79b2d4b175
192
src/action.rs
192
src/action.rs
|
|
@ -1,192 +0,0 @@
|
|||
use bevy::{
|
||||
diagnostic::{Diagnostics, FrameTimeDiagnosticsPlugin},
|
||||
prelude::*,
|
||||
};
|
||||
use bevy_rapier3d::prelude::{
|
||||
ExternalForce, Friction, NoUserData, RapierConfiguration, RapierPhysicsPlugin, Velocity,
|
||||
};
|
||||
|
||||
use crate::{
|
||||
bike::{CyberBikeBody, CyberWheel},
|
||||
input::InputState,
|
||||
};
|
||||
|
||||
#[derive(Debug, Resource, Reflect)]
|
||||
#[reflect(Resource)]
|
||||
pub struct MovementSettings {
|
||||
pub accel: f32,
|
||||
pub gravity: f32,
|
||||
pub sensitivity: f32,
|
||||
}
|
||||
|
||||
impl Default for MovementSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
sensitivity: 6.0,
|
||||
accel: 40.0,
|
||||
gravity: 7.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Resource, Reflect)]
|
||||
#[reflect(Resource)]
|
||||
struct CatControllerSettings {
|
||||
pub kp: f32,
|
||||
pub kd: f32,
|
||||
pub ki: f32,
|
||||
}
|
||||
|
||||
impl Default for CatControllerSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 17.0,
|
||||
kd: 4.0,
|
||||
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;
|
||||
}
|
||||
|
||||
fn gravity(
|
||||
mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
||||
settings: Res<MovementSettings>,
|
||||
) {
|
||||
let (xform, mut forces) = query.single_mut();
|
||||
let grav = xform.translation.normalize() * -settings.gravity;
|
||||
forces.force = grav;
|
||||
forces.torque = Vec3::ZERO;
|
||||
}
|
||||
|
||||
fn falling_cat(
|
||||
mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>,
|
||||
_diagnostics: Res<Diagnostics>,
|
||||
settings: Res<CatControllerSettings>,
|
||||
) {
|
||||
let (xform, mut forces, mut control_vars) = bike_query.single_mut();
|
||||
let wup = xform.translation.normalize();
|
||||
let bike_up = xform.up();
|
||||
|
||||
let wright = xform.forward().cross(wup).normalize();
|
||||
//let wback = wright.cross(wup);
|
||||
|
||||
let roll_error = wright.dot(bike_up);
|
||||
let pitch_error = wup.dot(xform.back());
|
||||
|
||||
// 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 mag =
|
||||
(settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
|
||||
forces.torque += xform.back() * mag;
|
||||
};
|
||||
|
||||
// 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
|
||||
.get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
|
||||
.and_then(|d| d.smoothed())
|
||||
.map(|x| x as u64)
|
||||
{
|
||||
if count % 30 == 0 {
|
||||
dbg!(roll_error, pitch_error, &control_vars);
|
||||
}
|
||||
}
|
||||
control_vars.decay();
|
||||
}
|
||||
|
||||
fn input_forces(
|
||||
settings: Res<MovementSettings>,
|
||||
input: Res<InputState>,
|
||||
mut cquery: Query<&mut Friction, With<CyberWheel>>,
|
||||
mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
||||
) {
|
||||
let (xform, mut forces) = bquery.single_mut();
|
||||
|
||||
// thrust
|
||||
let thrust = xform.forward() * input.throttle * settings.accel;
|
||||
forces.force += thrust;
|
||||
|
||||
// brake
|
||||
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();
|
||||
let force = ExternalForce::at_point(force, point, xform.translation);
|
||||
*forces += force;
|
||||
}
|
||||
|
||||
fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
|
||||
let (vels, mut forces) = query.single_mut();
|
||||
|
||||
if let Some(vel) = vels.linvel.try_normalize() {
|
||||
let v2 = vels.linvel.length_squared();
|
||||
forces.force -= vel * (v2 * 0.02);
|
||||
}
|
||||
}
|
||||
|
||||
pub struct CyberActionPlugin;
|
||||
impl Plugin for CyberActionPlugin {
|
||||
fn build(&self, app: &mut App) {
|
||||
app.init_resource::<MovementSettings>()
|
||||
.register_type::<MovementSettings>()
|
||||
.init_resource::<CatControllerSettings>()
|
||||
.register_type::<CatControllerSettings>()
|
||||
.add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
|
||||
.add_plugin(FrameTimeDiagnosticsPlugin::default())
|
||||
.add_startup_system(zero_gravity)
|
||||
.add_system(gravity.before("cat"))
|
||||
.add_system(falling_cat.label("cat"))
|
||||
.add_system(input_forces.label("iforces").after("cat"))
|
||||
.add_system(drag.label("drag").after("iforces"));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
use std::time::Instant;
|
||||
|
||||
use bevy::{
|
||||
prelude::{Component, ReflectResource, Resource},
|
||||
reflect::Reflect,
|
||||
};
|
||||
|
||||
#[derive(Debug, Resource)]
|
||||
pub(crate) struct ActionDebugInstant(pub Instant);
|
||||
|
||||
impl Default for ActionDebugInstant {
|
||||
fn default() -> Self {
|
||||
ActionDebugInstant(Instant::now())
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Resource, Reflect)]
|
||||
#[reflect(Resource)]
|
||||
pub struct MovementSettings {
|
||||
pub accel: f32,
|
||||
pub gravity: f32,
|
||||
pub sensitivity: f32,
|
||||
}
|
||||
|
||||
impl Default for MovementSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
sensitivity: 6.0,
|
||||
accel: 40.0,
|
||||
gravity: 7.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Resource, Reflect)]
|
||||
#[reflect(Resource)]
|
||||
pub struct CatControllerSettings {
|
||||
pub kp: f32,
|
||||
pub kd: f32,
|
||||
pub ki: f32,
|
||||
}
|
||||
|
||||
impl Default for CatControllerSettings {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 17.0,
|
||||
kd: 4.0,
|
||||
ki: 0.05,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Component, Debug, Clone, Copy)]
|
||||
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 {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
roll_integral: Default::default(),
|
||||
roll_prev: Default::default(),
|
||||
pitch_integral: Default::default(),
|
||||
pitch_prev: Default::default(),
|
||||
decay_factor: 0.9,
|
||||
roll_limit: 1.5,
|
||||
pitch_limit: 1.2,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl CatControllerState {
|
||||
pub fn decay(&mut self) {
|
||||
if self.roll_integral.abs() > 0.1 {
|
||||
self.roll_integral *= self.decay_factor;
|
||||
}
|
||||
if self.pitch_integral.abs() > 0.1 {
|
||||
self.pitch_integral *= self.decay_factor;
|
||||
}
|
||||
}
|
||||
|
||||
pub fn update_roll(&mut self, error: f32, dt: f32) -> (f32, f32) {
|
||||
let lim = self.roll_limit;
|
||||
self.roll_integral = (self.roll_integral + (error * dt)).min(lim).max(-lim);
|
||||
let derivative = (error - self.roll_prev) / dt;
|
||||
self.roll_prev = error;
|
||||
(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_limits(&mut self, roll: f32, pitch: f32) {
|
||||
self.roll_limit = roll;
|
||||
self.pitch_limit = pitch;
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,29 @@
|
|||
use bevy::{
|
||||
diagnostic::FrameTimeDiagnosticsPlugin,
|
||||
prelude::{App, IntoSystemDescriptor, Plugin},
|
||||
};
|
||||
use bevy_rapier3d::prelude::{NoUserData, RapierPhysicsPlugin};
|
||||
|
||||
mod components;
|
||||
mod systems;
|
||||
|
||||
pub use components::*;
|
||||
use systems::*;
|
||||
|
||||
pub struct CyberActionPlugin;
|
||||
impl Plugin for CyberActionPlugin {
|
||||
fn build(&self, app: &mut App) {
|
||||
app.init_resource::<MovementSettings>()
|
||||
.register_type::<MovementSettings>()
|
||||
.init_resource::<CatControllerSettings>()
|
||||
.init_resource::<ActionDebugInstant>()
|
||||
.register_type::<CatControllerSettings>()
|
||||
.add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
|
||||
.add_plugin(FrameTimeDiagnosticsPlugin::default())
|
||||
.add_startup_system(zero_gravity)
|
||||
.add_system(gravity.before("cat"))
|
||||
.add_system(falling_cat.label("cat"))
|
||||
.add_system(input_forces.label("iforces").after("cat"))
|
||||
.add_system(drag.label("drag").after("iforces"));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,109 @@
|
|||
#[cfg(feature = "inspector")]
|
||||
use std::time::Instant;
|
||||
|
||||
use bevy::prelude::{Query, Res, ResMut, Time, Transform, Vec3, With};
|
||||
use bevy_rapier3d::prelude::{ExternalForce, Friction, RapierConfiguration, Velocity};
|
||||
|
||||
#[cfg(feature = "inspector")]
|
||||
use super::ActionDebugInstant;
|
||||
use super::{CatControllerSettings, CatControllerState, MovementSettings};
|
||||
use crate::{
|
||||
bike::{CyberBikeBody, CyberWheel},
|
||||
input::InputState,
|
||||
};
|
||||
|
||||
/// Disable gravity in Rapier.
|
||||
pub(crate) fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
|
||||
config.gravity = Vec3::ZERO;
|
||||
}
|
||||
|
||||
/// The gravity vector points from the cyberbike to the center of the planet.
|
||||
pub(crate) fn gravity(
|
||||
mut query: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
||||
settings: Res<MovementSettings>,
|
||||
) {
|
||||
let (xform, mut forces) = query.single_mut();
|
||||
let grav = xform.translation.normalize() * -settings.gravity;
|
||||
forces.force = grav;
|
||||
forces.torque = Vec3::ZERO;
|
||||
}
|
||||
|
||||
/// PID-based controller for stabilizing attitude; keeps the cyberbike upright.
|
||||
pub(crate) fn falling_cat(
|
||||
mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CatControllerState)>,
|
||||
time: Res<Time>,
|
||||
settings: Res<CatControllerSettings>,
|
||||
#[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 wright = xform.forward().cross(wup).normalize();
|
||||
|
||||
let roll_error = wright.dot(bike_up);
|
||||
let pitch_error = wup.dot(xform.back());
|
||||
|
||||
// only try to correct roll if we're not totally vertical
|
||||
if pitch_error.abs() < 0.8 {
|
||||
let (derivative, integral) = control_vars.update_roll(roll_error, time.delta_seconds());
|
||||
let mag =
|
||||
(settings.kp * roll_error) + (settings.ki * integral) + (settings.kd * derivative);
|
||||
if mag.is_finite() {
|
||||
forces.torque += xform.back() * mag;
|
||||
}
|
||||
}
|
||||
|
||||
// we can do pitch correction any time, it's not coupled to roll
|
||||
let (derivative, integral) = control_vars.update_pitch(pitch_error, time.delta_seconds());
|
||||
let mag = (settings.kp * pitch_error) + (settings.ki * integral) + (settings.kd * derivative);
|
||||
if mag.is_finite() {
|
||||
forces.torque += wright * mag * 0.2;
|
||||
}
|
||||
|
||||
#[cfg(feature = "inspector")]
|
||||
{
|
||||
let now = Instant::now();
|
||||
if (now - debug_instant.0).as_millis() > 500 {
|
||||
dbg!(roll_error, pitch_error, &control_vars);
|
||||
debug_instant.0 = now;
|
||||
}
|
||||
}
|
||||
control_vars.decay();
|
||||
}
|
||||
|
||||
/// Apply forces to the cyberbike as a result of input.
|
||||
pub(crate) fn input_forces(
|
||||
settings: Res<MovementSettings>,
|
||||
input: Res<InputState>,
|
||||
mut cquery: Query<&mut Friction, With<CyberWheel>>,
|
||||
mut bquery: Query<(&Transform, &mut ExternalForce), With<CyberBikeBody>>,
|
||||
) {
|
||||
let (xform, mut forces) = bquery.single_mut();
|
||||
|
||||
// thrust
|
||||
let thrust = xform.forward() * input.throttle * settings.accel;
|
||||
forces.force += thrust;
|
||||
|
||||
// brake
|
||||
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();
|
||||
let force = ExternalForce::at_point(force, point, xform.translation);
|
||||
*forces += force;
|
||||
}
|
||||
|
||||
/// General velocity-based drag-force calculation; does not take orientation
|
||||
/// into account.
|
||||
pub(crate) fn drag(mut query: Query<(&Velocity, &mut ExternalForce), With<CyberBikeBody>>) {
|
||||
let (vels, mut forces) = query.single_mut();
|
||||
|
||||
if let Some(vel) = vels.linvel.try_normalize() {
|
||||
let v2 = vels.linvel.length_squared();
|
||||
forces.force -= vel * (v2 * 0.02);
|
||||
}
|
||||
}
|
||||
|
|
@ -10,7 +10,7 @@ use bevy_rapier3d::{
|
|||
},
|
||||
};
|
||||
|
||||
use crate::{action::CyberBikeControl, planet::PLANET_RADIUS};
|
||||
use crate::{action::CatControllerState, planet::PLANET_RADIUS};
|
||||
|
||||
type Meshterial<'a> = (
|
||||
ResMut<'a, Assets<Mesh>>,
|
||||
|
|
@ -127,7 +127,7 @@ fn spawn_cyberbike(
|
|||
});
|
||||
})
|
||||
.insert(CyberBikeBody)
|
||||
.insert(CyberBikeControl::default())
|
||||
.insert(CatControllerState::default())
|
||||
.id();
|
||||
|
||||
spawn_tires(&mut commands, &xform, bike, &wheel_conf, &mut meshterials);
|
||||
|
|
|
|||
Loading…
Reference in New Issue