diff --git a/esp32/include/gait/kinematic_constraints.h b/esp32/include/gait/kinematic_constraints.h new file mode 100644 index 0000000..23359a0 --- /dev/null +++ b/esp32/include/gait/kinematic_constraints.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include + +class KinConfig { + public: +#if defined(SPOTMICRO_ESP32) + static constexpr float coxa = 60.5f / 100.0f; + static constexpr float coxa_offset = 10.0f / 100.0f; + static constexpr float femur = 111.2f / 100.0f; + static constexpr float tibia = 118.5f / 100.0f; + static constexpr float L = 207.5f / 100.0f; + static constexpr float W = 78.0f / 100.0f; +#elif defined(SPOTMICRO_ESP32_MINI) + static constexpr float coxa = 35.0f / 100.0f; + static constexpr float coxa_offset = 0.0f / 100.0f; + static constexpr float femur = 52.0f / 100.0f; + static constexpr float tibia = 65.0f / 100.0f; + static constexpr float L = 120.0f / 100.0f; + static constexpr float W = 78.5f / 100.0f; +#elif defined(SPOTMICRO_YERTLE) + static constexpr float coxa = 35.0f / 100.0f; + static constexpr float coxa_offset = 0.0f; + static constexpr float femur = 130.0f / 100.0f; + static constexpr float tibia = 130.0f / 100.0f; + static constexpr float L = 240.0f / 100.0f; + static constexpr float W = 78.0f / 100.0f; +#endif + + static constexpr float mountOffsets[4][3] = { + {L / 2, 0, W / 2}, {L / 2, 0, -W / 2}, {-L / 2, 0, W / 2}, {-L / 2, 0, -W / 2}}; + + static constexpr float default_feet_positions[4][4] = { + {mountOffsets[0][0], -1, mountOffsets[0][2] + coxa, 1}, + {mountOffsets[1][0], -1, mountOffsets[1][2] - coxa, 1}, + {mountOffsets[2][0], -1, mountOffsets[2][2] + coxa, 1}, + {mountOffsets[3][0], -1, mountOffsets[3][2] - coxa, 1}, + }; + + // Max constants + static constexpr float max_roll = 15 * (float)M_PI_2; + static constexpr float max_pitch = 15 * (float)M_PI_2; + + static constexpr float max_body_shift_x = 0.25; + static constexpr float max_body_shift_z = 0.25; + + static constexpr float max_body_height = 1; + + static constexpr float max_step_length = 1; + + // Default constant + static constexpr float default_step_depth = 0.002; + static constexpr float default_body_height = 0.5; + static constexpr float default_step_height = 0.4; +}; diff --git a/esp32/include/gait/rest_state.h b/esp32/include/gait/rest_state.h index 03a9fb5..ed4561f 100644 --- a/esp32/include/gait/rest_state.h +++ b/esp32/include/gait/rest_state.h @@ -11,7 +11,7 @@ class RestState : public GaitState { body_state.phi = 0; body_state.psi = 0; body_state.xm = 0; - body_state.ym = getDefaultHeight() / 2; + body_state.ym = KinConfig::default_body_height / 2; body_state.zm = 0; body_state.updateFeet(default_feet_pos); } diff --git a/esp32/include/gait/state.h b/esp32/include/gait/state.h index 09c4a87..b08a599 100644 --- a/esp32/include/gait/state.h +++ b/esp32/include/gait/state.h @@ -1,22 +1,24 @@ #pragma once #include +#include #include struct gait_state_t { - float step_height; - float step_x; - float step_z; - float step_angle; - float step_velocity; - float step_depth; + float step_height {KinConfig::default_step_height}; + float step_x {0}; + float step_z {0}; + float step_angle {0}; + float step_velocity {0.5}; + float step_depth {KinConfig::default_step_depth}; }; class GaitState { protected: virtual const char *name() const = 0; 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}; + + gait_state_t gait_state; virtual void mapCommand(CommandMsg command) { this->gait_state.step_height = command.s1 / 2; @@ -28,8 +30,6 @@ class GaitState { } public: - virtual float getDefaultHeight() const { return 0.5f; } - virtual void begin() { ESP_LOGI("Gait Planner", "Starting %s", name()); } virtual void end() { ESP_LOGI("Gait Planner", "Ending %s", name()); } diff --git a/esp32/include/motion.h b/esp32/include/motion.h index bf9c6fc..86dd639 100644 --- a/esp32/include/motion.h +++ b/esp32/include/motion.h @@ -9,6 +9,7 @@ #include #include +#include #include #define DEFAULT_STATE false @@ -62,24 +63,24 @@ class MotionService { void handleInput(JsonVariant &root, int originId) { command.fromJson(root); - body_state.ym = command.h * 1.f - 0.5f; + body_state.ym = command.h - 0.5f; switch (motionState) { case MOTION_STATE::STAND: { - body_state.phi = command.rx * 10 * (float)M_PI_2; - body_state.psi = command.ry * 10 * (float)M_PI_2; - body_state.xm = command.ly / 4; - body_state.zm = command.lx / 4; + body_state.phi = command.rx * KinConfig::max_roll; + body_state.psi = command.ry * KinConfig::max_pitch; + body_state.xm = command.ly * KinConfig::max_body_shift_x; + body_state.zm = command.lx * KinConfig::max_body_shift_z; body_state.updateFeet(kinematics.default_feet_positions); break; } 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_height = KinConfig::default_step_height + command.s1; + gait_state.step_x = command.ly * KinConfig::max_step_length; + gait_state.step_z = -command.lx * KinConfig::max_step_length; gait_state.step_velocity = command.s; gait_state.step_angle = command.rx; - gait_state.step_depth = 0.002; + gait_state.step_depth = KinConfig::default_step_depth; break; } } @@ -160,7 +161,7 @@ class MotionService { 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}; + gait_state_t gait_state; 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};