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 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;
#else
#error "Must define either SPOTMICRO_ESP32 or SPOTMICRO_YERTLE"
@@ -53,6 +53,13 @@ class Kinematics {
body_state_t currentState;
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 ret = ESP_OK;
esp32/lib/ESP32-sveltekit/motion.h
+7 -4
View File
@@ -35,7 +35,7 @@ class MotionService {
socket.onSubscribe(ANGLES_EVENT,
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) {
@@ -75,7 +75,7 @@ class MotionService {
body_state.psi = command.ry / 8;
body_state.xm = command.ly / 2 / 100;
body_state.zm = command.lx / 2 / 100;
body_state.updateFeet(default_feet_positions);
body_state.updateFeet(kinematics.default_feet_positions);
break;
}
}
@@ -154,10 +154,13 @@ class MotionService {
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 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 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