From 70469576696f70932be83ac5e0baf896e3af234d Mon Sep 17 00:00:00 2001 From: Rune Harlyk Date: Sat, 9 Nov 2024 14:23:15 +0100 Subject: [PATCH] =?UTF-8?q?=F0=9F=A5=BD=20Adds=20rotation=20to=20the=20nat?= =?UTF-8?q?ive=20implementation?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- esp32/lib/ESP32-sveltekit/Gait/GaitState.h | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h index b220712..597a22a 100644 --- a/esp32/lib/ESP32-sveltekit/Gait/GaitState.h +++ b/esp32/lib/ESP32-sveltekit/Gait/GaitState.h @@ -259,18 +259,20 @@ class BezierState : public GaitState { void controller(const int index, body_state_t &body_state, std::function curve, float *arg) { + float delta_pos[3] = {0, 0, 0}; + float delta_rot[3] = {0, 0, 0}; + float length = step_length / 2.0f; float angle = std::atan2(gait_state.step_z, step_length) * 2; - float point[3] = {0, 0, 0}; - curve(length, angle, arg, phase_time, point); + curve(length, angle, arg, phase_time, delta_pos); - body_state.feet[index][0] += point[0]; - body_state.feet[index][1] += point[1]; - body_state.feet[index][2] += point[2]; + length = gait_state.step_angle * 2.0f; + angle = yawArc(default_feet_pos[index], body_state.feet[index]); + curve(length, angle, arg, phase_time, delta_rot); - // length = gait_state.step_angle * 2.0f; - // angle = yawArc(default_feet_pos[index], body_state.feet[index]); - // curve(length, angle, arg, phase_time, body_state.feet[index]); + body_state.feet[index][0] += delta_pos[0] + delta_rot[0] * 0.2; + if (step_length || gait_state.step_angle) body_state.feet[index][1] += delta_pos[1] + delta_rot[1] * 0.2; + body_state.feet[index][2] += delta_pos[2] + delta_rot[2] * 0.2; } static void stanceCurve(const float length, const float angle, const float *depth, const float phase,