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

🎨 Updates and simplifies command handling

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-01 18:41:59 +02:00
parent de3912ff10
commit e5bf10cdb0
14 changed files with 111 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/gait/bezier_state.h
+7 -6
View File
@@ -36,7 +36,7 @@ class BezierState : public GaitState {
public:
const char *name() const override { return "Bezier"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
this->mapCommand(command);
step_length = std::hypot(gait_state.step_x, gait_state.step_z);
if (gait_state.step_x < 0.0f) {
@@ -78,8 +78,8 @@ class BezierState : public GaitState {
float delta_pos[3] = {0};
float delta_rot[3] = {0};
float length = step_length / 2.0f;
float angle = std::atan2(gait_state.step_z, step_length) * 2;
float length = step_length * 0.5f;
float angle = std::atan2(gait_state.step_z, step_length);
curve(length, angle, arg, phase, delta_pos);
length = gait_state.step_angle * 2.0f;
@@ -107,9 +107,10 @@ class BezierState : public GaitState {
const float X_POLAR = std::cos(angle);
const float Z_POLAR = std::sin(angle);
const float t = std::clamp(phase, 1e-4f, 1.f - 1e-4f);
float phase_power = 1.0f;
float inv_phase_power = std::pow(1.0f - phase, 11);
const float one_minus_phase = 1.0f - phase;
float inv_phase_power = std::pow(1.0f - t, 11);
const float one_minus_phase = 1.0f - t;
for (int i = 0; i < 12; i++) {
float b = COMBINATORIAL_VALUES[i] * phase_power * inv_phase_power;
@@ -122,7 +123,7 @@ class BezierState : public GaitState {
}
}
static float yawArc(const float feet_pos[4], const float *current_pos) {
static float yawArc(const float feet_pos[3], const float *current_pos) {
const float foot_mag = std::hypot(feet_pos[0], feet_pos[2]);
const float foot_dir = std::atan2(feet_pos[2], feet_pos[0]);
const float offsets[] = {current_pos[0] - feet_pos[0], current_pos[1] - feet_pos[1],
esp32/include/gait/crawl_state.h
+1 -1
View File
@@ -27,7 +27,7 @@ class EightPhaseWalkState : public PhaseGaitState {
}
}
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/four_phase_trot_state.h
+1 -1
View File
@@ -22,7 +22,7 @@ class FourPhaseWalkState : public PhaseGaitState {
}
}
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
return PhaseGaitState::step(body_state, command, dt);
}
};
esp32/include/gait/phase_state_base.h
+1 -1
View File
@@ -14,7 +14,7 @@ class PhaseGaitState : public GaitState {
uint8_t contact_phases[4][8];
float shifts[4][3];
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
mapCommand(command);
this->dt = dt;
updatePhase();
esp32/include/gait/rest_state.h
+1 -1
View File
@@ -6,7 +6,7 @@ class RestState : public GaitState {
protected:
const char *name() const override { return "Rest"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
body_state.omega = 0;
body_state.phi = 0;
body_state.psi = 0;
esp32/include/gait/stand_state.h
+1 -1
View File
@@ -6,7 +6,7 @@ class StandState : public GaitState {
protected:
const char *name() const override { return "Stand"; }
void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) override {
void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) override {
body_state.omega = 0;
body_state.phi = command.rx / 8;
body_state.psi = command.ry / 8;
esp32/include/gait/state.h
+9 -15
View File
@@ -1,6 +1,7 @@
#pragma once
#include <kinematics.h>
#include <message_types.h>
struct gait_state_t {
float step_height;
@@ -11,23 +12,18 @@ struct gait_state_t {
float step_depth;
};
struct ControllerCommand {
int stop;
float lx, ly, rx, ry, h, s, s1;
};
class GaitState {
protected:
virtual const char *name() const = 0;
float default_feet_pos[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}};
static constexpr const float (&default_feet_pos)[4][4] = Kinematics::default_feet_positions;
gait_state_t gait_state = {0.4, 0, 0, 0, 1, 0.002};
void mapCommand(ControllerCommand command) {
this->gait_state.step_height = 0.4 + (command.s1 / 128 + 1) / 2;
this->gait_state.step_x = command.ly / 128;
this->gait_state.step_z = -command.lx / 128;
this->gait_state.step_velocity = command.s / 128 + 1;
this->gait_state.step_angle = command.rx / 128;
virtual void mapCommand(CommandMsg command) {
this->gait_state.step_height = command.s1 / 2;
this->gait_state.step_x = command.ly;
this->gait_state.step_z = -command.lx;
this->gait_state.step_velocity = command.s;
this->gait_state.step_angle = command.rx;
this->gait_state.step_depth = 0.002;
}
@@ -38,7 +34,5 @@ class GaitState {
virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); }
virtual void step(body_state_t &body_state, ControllerCommand command, float dt = 0.02f) {
this->mapCommand(command);
}
virtual void step(body_state_t &body_state, CommandMsg command, float dt = 0.02f) { this->mapCommand(command); }
};
esp32/include/kinematics.h
+2 -2
View File
@@ -30,8 +30,8 @@ class Kinematics {
static constexpr float L = 207.5f / 100.0f;
static constexpr float W = 78.0f / 100.0f;
#elif defined(SPOTMICRO_ESP32_MINI)
static constexpr float l1 = 0.0f / 100.0f;
static constexpr float l2 = 0.0f / 100.0f;
static constexpr float l1 = 0.01f / 100.0f;
static constexpr float l2 = 0.01f / 100.0f;
static constexpr float l3 = 52.0f / 100.0f;
static constexpr float l4 = 65.0f / 100.0f;
esp32/include/message_types.h
+28
View File
@@ -0,0 +1,28 @@
#pragma once
#include <ArduinoJson.h>
struct CommandMsg {
float lx, ly, rx, ry, h, s, s1;
friend void toJson(JsonVariant v, CommandMsg const &c) {
JsonArray arr = v.to<JsonArray>();
arr.add(c.lx);
arr.add(c.ly);
arr.add(c.rx);
arr.add(c.ry);
arr.add(c.h);
arr.add(c.s);
arr.add(c.s1);
}
void fromJson(JsonVariantConst o) {
JsonArrayConst arr = o.as<JsonArrayConst>();
lx = arr[0].as<float>();
ly = arr[1].as<float>();
rx = arr[2].as<float>();
ry = arr[3].as<float>();
h = arr[4].as<float>();
s = arr[5].as<float>();
s1 = arr[6].as<float>();
}
};
esp32/include/motion.h
+21 -14
View File
@@ -10,6 +10,7 @@
#include <gait/state.h>
#include <gait/crawl_state.h>
#include <gait/bezier_state.h>
#include <message_types.h>
#define DEFAULT_STATE false
#define ANGLES_EVENT "angles"
@@ -57,26 +58,29 @@ class MotionService {
}
void handleInput(JsonVariant &root, int originId) {
JsonArray array = root.as<JsonArray>();
command.lx = array[1];
command.ly = array[2];
command.rx = array[3];
command.ry = array[4];
command.h = array[5];
command.s = array[6];
command.s1 = array[7];
command.fromJson(root);
body_state.ym = (command.h + 127.f) * 0.35f / 100;
body_state.ym = command.h * 1.f - 0.5f;
switch (motionState) {
case MOTION_STATE::STAND: {
body_state.phi = command.rx / 8;
body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100;
body_state.phi = command.rx; // * 0.254f;
body_state.psi = command.ry; // * 0.254f;
body_state.xm = command.ly / 4;
body_state.zm = command.lx / 4;
body_state.updateFeet(kinematics.default_feet_positions);
break;
}
case MOTION_STATE::CRAWL:
case MOTION_STATE::WALK: {
gait_state.step_height = 0.4 + (command.s1 + 1) / 2;
gait_state.step_x = command.ly;
gait_state.step_z = -command.lx;
gait_state.step_velocity = command.s;
gait_state.step_angle = command.rx;
gait_state.step_depth = 0.002;
break;
}
}
}
@@ -139,7 +143,7 @@ class MotionService {
private:
ServoController *_servoController;
Kinematics kinematics;
ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
friend class GaitState;
@@ -149,7 +153,10 @@ class MotionService {
MOTION_STATE motionState = MOTION_STATE::DEACTIVATED;
unsigned long _lastUpdate;
body_state_t target_body_state = {0, 0, 0, 0, 0, 0};
body_state_t body_state = {0, 0, 0, 0, 0, 0};
gait_state_t gait_state = {12, 0, 0, 0, 1, 0.002};
float new_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};
esp32/test/gait_performance.cpp
+3 -2
View File
@@ -7,7 +7,7 @@ void test_gaitPlanner_calculateStep_time() {
BezierState gaitPlanner;
body_state_t body_state = {
128, 0, 0, 0, 0, 0, {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}};
ControllerCommand command = {0, 0, 0, 0, 0, 0, 0, 0};
CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
const int num_steps = 1000;
unsigned long start = millis();
@@ -20,7 +20,8 @@ void test_gaitPlanner_calculateStep_time() {
unsigned long max_duration = num_steps / 2; // Minimum 0.5 ms per step
char message[50];
snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration, duration / num_steps);
snprintf(message, sizeof(message), "The step calculation took: %lu ms (%lu ms per iter)", duration,
duration / num_steps);
ESP_LOGI("Test planner", message);
TEST_ASSERT_MESSAGE(duration <= max_duration, message);
}