From 413ac493dfc9e8f8670a1ff224f330398960c286 Mon Sep 17 00:00:00 2001 From: Joe Ardent Date: Tue, 22 Mar 2022 20:24:01 -0700 Subject: [PATCH] Add PID for pitch too. --- .gitignore | 1 + Cargo.lock | 32 +++++++++++----------- src/action.rs | 76 +++++++++++++++++++++++++++++++++++++++------------ src/bike.rs | 6 ++-- 4 files changed, 80 insertions(+), 35 deletions(-) diff --git a/.gitignore b/.gitignore index fe6cd2a..5631d26 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ /target .#* +*.*undo-tree* diff --git a/Cargo.lock b/Cargo.lock index ca6f446..0eb9194 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1001,9 +1001,9 @@ dependencies = [ [[package]] name = "crossbeam-channel" -version = "0.5.3" +version = "0.5.4" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "fdbfe11fe19ff083c48923cf179540e8cd0535903dc35e178a1fdeeb59aef51f" +checksum = "5aaa7bd5fb665c6864b5f963dd9097905c54125909c7aa94c9e18507cdbe6c53" dependencies = [ "cfg-if 1.0.0", "crossbeam-utils", @@ -1165,9 +1165,9 @@ dependencies = [ [[package]] name = "erased-serde" -version = "0.3.19" +version = "0.3.20" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2df1bebfbf6d5e523e871439e5bd41d80a617b5a796cca0d616b9b30edf3c04d" +checksum = "ad132dd8d0d0b546348d7d86cb3191aad14b34e5f979781fc005c80d4ac67ffd" dependencies = [ "serde", ] @@ -1590,9 +1590,9 @@ checksum = "e2abad23fbc42b3700f2f279844dc832adb2b2eb069b2df918f455c4e18cc646" [[package]] name = "libc" -version = "0.2.120" +version = "0.2.121" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ad5c14e80759d0939d013e6ca49930e59fc53dd8e5009132f76240c179380c09" +checksum = "efaa7b300f3b5fe8eb6bf21ce3895e1751d9665086af2d64b42f19701015ff4f" [[package]] name = "libloading" @@ -1637,9 +1637,9 @@ dependencies = [ [[package]] name = "log" -version = "0.4.14" +version = "0.4.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "51b9bbe6c47d51fc3e1a9b945965946b4c44142ab8792c50835a980d362c2710" +checksum = "6389c490849ff5bc16be905ae24bc913a9c8892e19b2341dbc175e14c341c2b8" dependencies = [ "cfg-if 1.0.0", ] @@ -1726,9 +1726,9 @@ checksum = "e53debba6bda7a793e5f99b8dacf19e626084f525f7829104ba9898f367d85ff" [[package]] name = "mio" -version = "0.8.1" +version = "0.8.2" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7ba42135c6a5917b9db9cd7b293e5409e1c6b041e6f9825e92e55a894c63b6f8" +checksum = "52da4364ffb0e4fe33a9841a98a3f3014fb964045ce4f7a45a398243c8d6b0c9" dependencies = [ "libc", "log", @@ -2167,9 +2167,9 @@ checksum = "9145ac0af1d93c638c98c40cf7d25665f427b2a44ad0a99b1dccf3e2f25bb987" [[package]] name = "quote" -version = "1.0.15" +version = "1.0.16" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "864d3e96a899863136fc6e99f3d7cae289dafe43bf2c5ac19b70df7210c0a145" +checksum = "b4af2ec4714533fcdf07e886f17025ace8b997b9ce51204ee69b6da831c3da57" dependencies = [ "proc-macro2", ] @@ -2305,9 +2305,9 @@ checksum = "a0d463f2884048e7153449a55166f91028d5b0ea53c79377099ce4e8cf0cf9bb" [[package]] name = "redox_syscall" -version = "0.2.11" +version = "0.2.12" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8380fe0152551244f0747b1bf41737e0f8a74f97a14ccefd1148187271634f3c" +checksum = "8ae183fc1b06c149f0c1793e1eb447c8b04bfe46d48e9e48bfb8d2d7ed64ecf0" dependencies = [ "bitflags", ] @@ -2601,9 +2601,9 @@ checksum = "8fb1df15f412ee2e9dfc1c504260fa695c1c3f10fe9f4a6ee2d2184d7d6450e2" [[package]] name = "syn" -version = "1.0.88" +version = "1.0.89" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ebd69e719f31e88618baa1eaa6ee2de5c9a1c004f1e9ecdb58e8352a13f20a01" +checksum = "ea297be220d52398dcc07ce15a209fce436d361735ac1db700cab3b6cdfb9f54" dependencies = [ "proc-macro2", "quote", diff --git a/src/action.rs b/src/action.rs index e87413b..a84a288 100644 --- a/src/action.rs +++ b/src/action.rs @@ -34,36 +34,78 @@ fn gravity( fn falling_cat_pid( mut bike_query: Query<( &Transform, + &RigidBodyVelocityComponent, &mut RigidBodyForcesComponent, &mut CyberBikeControl, )>, ) { - let (xform, mut forces, mut control_vars) = bike_query.single_mut(); + let (xform, vels, mut forces, mut control_vars) = bike_query.single_mut(); let up = xform.translation.normalize(); + let cam_forward = xform.forward(); let cam_right = xform.right(); - let cos = up.dot(cam_right); - let error = if cos.is_normal() { -cos } else { 0.0 }; + let pitch_cos = up.dot(cam_forward); + let roll_cos = up.dot(cam_right); - let derivative = error - control_vars.previous; - control_vars.previous = error; + let mut torque = Vec3::ZERO; - let integral = (control_vars.sum * 0.8) + error; - control_vars.sum = integral; + // roll + { + let roll_error = if roll_cos.is_normal() { -roll_cos } else { 0.0 }; + let derivative = roll_error - control_vars.prev_roll_error; + control_vars.prev_roll_error = roll_error; - let proportional = error; + let integral = (control_vars.roll_sum * 0.8) + roll_error; + control_vars.roll_sum = integral; - let kp = 7.1; - let ki = 0.8; - let kd = 5.1; - let mag = (kp * proportional) + (ki * integral) + (kd * derivative); + //dbg!(integral); - let torque = xform.back() * mag; + let proportional = roll_error; - if xform.forward().dot(up).abs() < 0.55 { - forces.torque = torque.into(); - } else { - forces.torque = Vec3::ZERO.into(); + let kp = 7.1; + let ki = 0.8; + let kd = 5.1; + let mag = (kp * proportional) + (ki * integral) + (kd * derivative); + + let angvel = vels.angvel.as_ref(); + let roll_rate = cam_forward.dot(Vec3::from_slice(angvel)).abs(); + dbg!(roll_rate); + if pitch_cos.abs() < 0.85 && roll_rate < 1.0 { + torque += xform.back() * mag; + } } + + // pitch + { + let pitch_error = if pitch_cos.is_normal() { + -pitch_cos + } else { + 0.0 + }; + + let derivative = pitch_error - control_vars.prev_pitch_error; + control_vars.prev_pitch_error = pitch_error; + + let integral = (control_vars.pitch_sum * 0.7) + pitch_error; + control_vars.pitch_sum = integral; + + dbg!(integral); + + let proportional = pitch_error; + + let kp = 7.1; + let ki = 0.3; + let kd = 4.1; + let mag = (kp * proportional) + (ki * integral) + (kd * derivative); + + let angvel = vels.angvel.as_ref(); + let pitch_rate = cam_right.dot(Vec3::from_slice(angvel)).abs(); + dbg!(pitch_rate); + if roll_cos.abs() < 0.85 && pitch_rate < 2.0 { + torque += xform.left() * mag; + } + } + + forces.torque = torque.into(); } fn drag( diff --git a/src/bike.rs b/src/bike.rs index 13c9e4f..bd8410a 100644 --- a/src/bike.rs +++ b/src/bike.rs @@ -16,8 +16,10 @@ pub struct CyberBikeModel; #[derive(Component, Debug, Default, Clone, Copy)] pub struct CyberBikeControl { - pub sum: f32, - pub previous: f32, + pub roll_sum: f32, + pub prev_roll_error: f32, + pub pitch_sum: f32, + pub prev_pitch_error: f32, } const BIKE_BODY_GROUP: InteractionGroups = InteractionGroups::new(0b01, 0b01);