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

checkpoint

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>) { fn zero_gravity(mut config: ResMut<RapierConfiguration>) {
config.gravity = Vec3::ZERO; config.gravity = Vec3::ZERO;
} }
@ -41,9 +49,10 @@ fn gravity(
forces.force = grav; forces.force = grav;
} }
fn falling_cat_pid( fn falling_cat(
mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>, mut bike_query: Query<(&Transform, &mut ExternalForce, &mut CyberBikeControl)>,
diagnostics: Res<Diagnostics>, diagnostics: Res<Diagnostics>,
settings: Res<CatControllerSettings>,
) { ) {
let (xform, mut forces, mut control_vars) = bike_query.single_mut(); let (xform, mut forces, mut control_vars) = bike_query.single_mut();
let up = Vec3::Y; let up = Vec3::Y;
@ -51,19 +60,17 @@ fn falling_cat_pid(
let torque = bike_up.cross(up).normalize_or_zero(); let torque = bike_up.cross(up).normalize_or_zero();
let cos = up.dot(bike_up); 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 error = 1.0 - cos;
let derivative = error - control_vars.prev_error; let derivative = error - control_vars.prev_error;
control_vars.prev_error = error; control_vars.prev_error = error;
// this integral term is not an integral, it's more like a weighted moving sum // this integral term is not an integral, it's more like a weighted moving sum
let integral = (control_vars.error_sum + error) * 0.8; let weighted_sum = control_vars.error_sum + error;
control_vars.error_sum = integral; //.min(2.0).max(-2.0); control_vars.error_sum = weighted_sum * 0.8;
let kp = 13.1; let mag = (settings.kp * error) + (settings.kws * weighted_sum) + (settings.kd * derivative);
let ki = 1.1;
let kd = 8.1;
let mag = (kp * error) + (ki * integral) + (kd * derivative);
if let Some(count) = diagnostics if let Some(count) = diagnostics
.get(FrameTimeDiagnosticsPlugin::FRAME_COUNT) .get(FrameTimeDiagnosticsPlugin::FRAME_COUNT)
@ -71,7 +78,7 @@ fn falling_cat_pid(
.map(|x| x as u64) .map(|x| x as u64)
{ {
if count % 30 == 0 { if count % 30 == 0 {
dbg!(&control_vars, mag); dbg!(&control_vars, mag, cos);
} }
} }
@ -112,11 +119,13 @@ pub struct CyberActionPlugin;
impl Plugin for CyberActionPlugin { impl Plugin for CyberActionPlugin {
fn build(&self, app: &mut App) { fn build(&self, app: &mut App) {
app.init_resource::<MovementSettings>() app.init_resource::<MovementSettings>()
.init_resource::<CatControllerSettings>()
.register_type::<CatControllerSettings>()
.add_plugin(RapierPhysicsPlugin::<NoUserData>::default()) .add_plugin(RapierPhysicsPlugin::<NoUserData>::default())
.add_plugin(FrameTimeDiagnosticsPlugin::default()) .add_plugin(FrameTimeDiagnosticsPlugin::default())
.add_startup_system(zero_gravity) .add_startup_system(zero_gravity)
.add_system(gravity.before("cat")) .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(input_forces.label("iforces").after("cat"))
.add_system(drag.label("drag").after("iforces")); .add_system(drag.label("drag").after("iforces"));
} }