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

👔 Calculate default feet positions from kinematics

Browse Source
This commit is contained in:
Rune Harlyk
2025-07-02 22:53:58 +02:00
parent 4101ad033c
commit 632f603fda
2 changed files with 15 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/kinematics.h
+8 -1
View File
@@ -35,7 +35,7 @@ class Kinematics {
static constexpr float l3 = 130.0f / 100.0f; static constexpr float l3 = 130.0f / 100.0f;
static constexpr float l4 = 130.0f / 100.0f; static constexpr float l4 = 130.0f / 100.0f;
static constexpr float L = 207.5f / 100.0f; static constexpr float L = 240.0f / 100.0f;
static constexpr float W = 78.0f / 100.0f; static constexpr float W = 78.0f / 100.0f;
#else #else
#error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE" #error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE"
@@ -53,6 +53,13 @@ class Kinematics {
body_state_t currentState; body_state_t currentState;
public: public:
static constexpr float default_feet_positions[4][4] = {
{mountOffsets[0][0], -1, mountOffsets[0][2] + l1, 1},
{mountOffsets[1][0], -1, mountOffsets[1][2] - l1, 1},
{mountOffsets[2][0], -1, mountOffsets[2][2] + l1, 1},
{mountOffsets[3][0], -1, mountOffsets[3][2] - l1, 1},
};
esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) { esp_err_t calculate_inverse_kinematics(const body_state_t body_state, float result[12]) {
esp_err_t ret = ESP_OK; esp_err_t ret = ESP_OK;
esp32/lib/ESP32-sveltekit/motion.h
+7 -4
View File
@@ -35,7 +35,7 @@ class MotionService {
socket.onSubscribe(ANGLES_EVENT, socket.onSubscribe(ANGLES_EVENT,
std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2)); std::bind(&MotionService::syncAngles, this, std::placeholders::_1, std::placeholders::_2));
body_state.updateFeet(default_feet_positions); body_state.updateFeet(kinematics.default_feet_positions);
} }
void anglesEvent(JsonObject &root, int originId) { void anglesEvent(JsonObject &root, int originId) {
@@ -75,7 +75,7 @@ class MotionService {
body_state.psi = command.ry / 8; body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100; body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100; body_state.zm = command.lx / 2 / 100;
body_state.updateFeet(default_feet_positions); body_state.updateFeet(kinematics.default_feet_positions);
break; break;
} }
} }
@@ -154,10 +154,13 @@ class MotionService {
float 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};
float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1}; float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
float default_feet_positions[4][4] = {{1, -1, 0.7, 1}, {1, -1, -0.7, 1}, {-1, -1, 0.7, 1}, {-1, -1, -0.7, 1}}; #if defined(SPOTMICRO_ESP32)
float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145}; float rest_angles[12] = {0, 90, -145, 0, 90, -145, 0, 90, -145, 0, 90, -145};
float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; float calibration_angles[12] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
#elif defined(SPOTMICRO_YERTLE)
float rest_angles[12] = {0, 45, -45, 0, 45, -45, 0, 45, -45, 0, 45, -45};
float calibration_angles[12] = {0, 90, 0, 0, 90, 0, 0, 90, 0, 0, 90, 0};
#endif
}; };
#endif #endif