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

✨ Makes stand imu compensating

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-06 20:34:06 +02:00
committed by Rune Harlyk
parent bd8c8fd988
commit ae98ba76f7
4 changed files with 13 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/motion_states/state.h
+10 -2
View File
@@ -9,13 +9,17 @@ class MotionState {
static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
body_state_t target_body_state;
static constexpr float default_smoothing_factor = 0.06f;
float omega_offset = 0, psi_offset = 0;
void lerpToBody(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
void lerpToBody(body_state_t& body_state, const bool imuCompensate = false,
const float smoothing_factor = default_smoothing_factor) {
body_state.xm = lerp(body_state.xm, target_body_state.xm, smoothing_factor);
body_state.ym = lerp(body_state.ym, target_body_state.ym, smoothing_factor);
body_state.zm = lerp(body_state.zm, target_body_state.zm, smoothing_factor);
body_state.phi = lerp(body_state.phi, target_body_state.phi, smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi, smoothing_factor);
body_state.psi = lerp(body_state.psi, target_body_state.psi - imuCompensate * psi_offset, smoothing_factor);
body_state.omega =
lerp(body_state.omega, target_body_state.omega - imuCompensate * omega_offset, smoothing_factor);
}
void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
@@ -25,6 +29,10 @@ class MotionState {
}
public:
void updateImuOffsets(const float omega_offset, const float psi_offset) {
this->omega_offset = omega_offset * RAD2DEG_F;
this->psi_offset = psi_offset * RAD2DEG_F;
}
virtual ~MotionState() {}
virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }