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

checkpoint

Browse source
This commit is contained in:
Joe Ardent 2023-01-24 19:50:33 -08:00
parent 9b12967abd
commit 3e9b079b58
1 changed files with 19 additions and 10 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

29
src/action.rs
View file

@ -28,6 +28,14 @@ impl Default for MovementSettings {
}
}
#[derive(Debug, Default, Resource, Reflect)]
#[reflect(Resource)]
struct CatControllerSettings {
pub kp: f32,
pub kd: f32,
pub kws: f32,
}
fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
config.gravity = Vec3::ZERO;
}
@ -41,9 +49,10 @@ fn gravity(
forces.force = grav;
}
fn falling_cat_pid(
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 up = Vec3::Y;
@ -51,19 +60,17 @@ fn falling_cat_pid(
let torque = bike_up.cross(up).normalize_or_zero();
let cos = up.dot(bike_up);
let cos = if cos.is_finite() { cos } else { 0.0 };
let cos = if cos.is_finite() { cos } else { 1.0 };
let error = 1.0 - cos;
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 integral = (control_vars.error_sum + error) * 0.8;
control_vars.error_sum = integral; //.min(2.0).max(-2.0);
let weighted_sum = control_vars.error_sum + error;
control_vars.error_sum = weighted_sum * 0.8;
let kp = 13.1;
let ki = 1.1;
let kd = 8.1;
let mag = (kp * error) + (ki * integral) + (kd * derivative);
let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative);
if let Some(count) = diagnostics
.get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
@ -71,7 +78,7 @@ fn falling_cat_pid(
.map(|x| x as u64)
{
if count % 30 == 0 {
dbg!(&control_vars, mag);
dbg!(&control_vars, mag, cos);
}
}
@ -112,11 +119,13 @@ pub struct CyberActionPlugin;
impl Plugin for CyberActionPlugin {
fn build(&self, app: &mut App) {
app.init_resource::<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_pid.label("cat"))
.add_system(falling_cat.label("cat"))
.add_system(input_forces.label("iforces").after("cat"))
.add_system(drag.label("drag").after("iforces"));
}