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.h
+1
View File
@@ -107,6 +107,7 @@ class MotionService {
bool updateMotion() { bool updateMotion() {
if (!state) return false; if (!state) return false;
state->updateImuOffsets(_peripherals->angleY(), _peripherals->angleX());
state->step(body_state); state->step(body_state);
kinematics.calculate_inverse_kinematics(body_state, new_angles); kinematics.calculate_inverse_kinematics(body_state, new_angles);
esp32/include/motion_states/stand_state.h
+1 -1
View File
@@ -26,7 +26,7 @@ class StandState : public MotionState {
} }
void step(body_state_t &body_state, float dt = 0.02f) override { void step(body_state_t &body_state, float dt = 0.02f) override {
lerpToBody(body_state); lerpToBody(body_state, true);
updateFeet(body_state); updateFeet(body_state);
} }
}; };
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; static constexpr const float (&default_feet_pos)[4][4] = KinConfig::default_feet_positions;
body_state_t target_body_state; body_state_t target_body_state;
static constexpr float default_smoothing_factor = 0.06f; 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.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.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.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.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) { void updateFeet(body_state_t& body_state, const float smoothing_factor = default_smoothing_factor) {
@@ -25,6 +29,10 @@ class MotionState {
} }
public: 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 ~MotionState() {}
virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); } virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); }
esp32/include/motion_states/walk_state.h
+1 -1
View File
@@ -23,7 +23,7 @@ class WalkState : public MotionState {
float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f}; float phase_offset[4] = {0.f, 0.5f, 0.5f, 0.f};
float stand_offset = 0.6f; float stand_offset = 0.6f;
float step_length = 0.0f; float step_length = 0.0f;
float speed_factor = 1; float speed_factor = 2;
gait_state_t gait_state; gait_state_t gait_state;
gait_state_t target_gait_state; gait_state_t target_gait_state;