Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

✨ Makes fsm states by time aware

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-08 22:39:53 +02:00
parent 59bb1d9579
commit cd802f1c22
1 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/motion.h
+6 -1
View File
@@ -107,8 +107,11 @@ class MotionService {
bool updateMotion() { bool updateMotion() {
if (!state) return false; if (!state) return false;
unsigned long now = millis();
float dt = (now - lastUpdate) / 1000.0f;
lastUpdate = now;
state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX()); state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX());
state->step(body_state); state->step(body_state, dt);
kinematics.calculate_inverse_kinematics(body_state, new_angles); kinematics.calculate_inverse_kinematics(body_state, new_angles);
return update_angles(new_angles, 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 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}; float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
unsigned long lastUpdate = millis();
}; };
#endif #endif