From cd802f1c22b57f07d276ef026323cc94ed67af96 Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Mon, 8 Sep 2025 22:39:53 +0200 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Makes=20fsm=20states=20by=20time=20?= =?UTF-8?q?aware?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/include/motion.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/esp32/include/motion.h b/esp32/include/motion.h index 0f5f8b0..0bb9871 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -107,8 +107,11 @@ class MotionService { bool updateMotion() { if (!state) return false; + unsigned long now = millis(); + float dt = (now - lastUpdate) / 1000.0f; + lastUpdate = now; state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX()); - state->step(body_state); + state->step(body_state, dt); kinematics.calculate_inverse_kinematics(body_state, new_angles); return update_angles(new_angles, angles); @@ -149,6 +152,8 @@ class MotionService { float angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1}; + + unsigned long lastUpdate = millis(); }; #endif