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

🪄 Simplifies kin code

Browse Source
This commit is contained in:
Rune Harlyk
2024-05-19 19:36:05 +02:00
committed by Rune Harlyk
parent 90e72cbacc
commit 6626c2e274
2 changed files with 26 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/lib/ESP32-sveltekit/Kinematics.h
+18 -13
View File
@@ -41,28 +41,33 @@ public:
L = 140;
W = 75;
float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
dspm::Mat Ix(Ix_data, 4, 4);
}
~Kinematics(){}
esp_err_t calculate_inverse_kinematics(float lp[4][4], position_t p, float result[12]) {
Tlf.clear();
Trf.clear();
Tlb.clear();
Trb.clear();
float Ix_data[] = {-1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
dspm::Mat Ix(Ix_data, 4, 4);
esp_err_t res = bodyIK(p);
for (int i = 0; i < 4; ++i) {
dspm::Mat temp(lp[i], 4, 1);
dspm::Mat result_vec(4, 1);
dspm::Mat result_vec(4, 4);
result_vec = (Tlf.inverse() * dspm::Mat(lp[0], 4, 1));
legIK(result_vec.data, result);
if (i == 1 || i == 3) {
result_vec = Ix * ((i == 1 ? Trf.inverse() : Trb.inverse()) * temp);
} else {
result_vec = (i == 0 ? Tlf.inverse() : Tlb.inverse()) * temp;
}
result_vec = Ix * (Trf.inverse() * dspm::Mat(lp[1], 4, 1));
legIK(result_vec.data, result + 3);
legIK(result_vec.data, &result[i * 3]);
}
result_vec = (Tlb.inverse() * dspm::Mat(lp[2], 4, 1));
legIK(result_vec.data, result + 6);
result_vec = Ix * (Trb.inverse() * dspm::Mat(lp[3], 4, 1));
legIK(result_vec.data, result + 9);
return res;
}
esp32/lib/ESP32-sveltekit/MotionService.h
+8 -2
View File
@@ -106,7 +106,13 @@ class MotionService
{-100, -100, 100, 1},
{-100, -100, -100, 1}
};
position_t p = {0, 0, 0, 0, static_cast<float>(input[5]), 0, input[0]};
float lx = static_cast<float>(input[1]);
float ly = static_cast<float>(input[2]);
float rx = static_cast<float>(input[3]);
float ry = static_cast<float>(input[4]);
float h = static_cast<float>(input[5]);
float s = static_cast<float>(input[6]);
position_t p = {0, rx / 4, ry / 4, ly / 2, h, lx / 2, input[0]};
float new_angles[12] = {0,};
float dir[12] = {-1, -1, -1, 1, -1, -1, -1, -1, -1, 1, -1, -1};
@@ -120,7 +126,7 @@ class MotionService
}
}
if (updated) {
ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %f", new_angles[0], new_angles[1], new_angles[2], new_angles[3], new_angles[4], new_angles[5], new_angles[6], new_angles[7], new_angles[8], new_angles[9], new_angles[10], new_angles[11]);
ESP_LOGI("MotionService", "New angles: %f %f %f %f %f %f %f %f %f %f %f %f", angles[0], angles[1], angles[2], angles[3], angles[4], angles[5], angles[6], angles[7], angles[8], angles[9], angles[10], angles[11]);
}
break;
}