Add PID for pitch too.
This commit is contained in:
Joe Ardent
parent
c37cf0a6b1
commit
413ac493df
4 changed files with 80 additions and 35 deletions
1
.gitignore
vendored
1
.gitignore
vendored
|
|
@ -1,2 +1,3 @@
|
|||
/target
|
||||
.#*
|
||||
*.*undo-tree*
|
||||
|
|
|
|||
32
Cargo.lock
generated
32
Cargo.lock
generated
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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(
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Reference in a new issue