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

🎨 Use std for min and max

Browse Source
This commit is contained in:
Rune Harlyk
2025-09-10 20:21:56 +02:00
parent 3a401abfab
commit 923ea17702
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esp32/include/kinematics.h
+2 -2
View File
@@ -184,13 +184,13 @@ class Kinematics {
} }
inline void legIK(float x, float y, float z, float out[3]) { inline void legIK(float x, float y, float z, float out[3]) {
float F = sqrt(max(0.0f, x * x + y * y - coxa * coxa)); float F = sqrt(std::max(0.0f, x * x + y * y - coxa * coxa));
float G = F - coxa_offset; float G = F - coxa_offset;
float H = sqrt(G * G + z * z); float H = sqrt(G * G + z * z);
float theta1 = -atan2f(y, x) - atan2f(F, -coxa); float theta1 = -atan2f(y, x) - atan2f(F, -coxa);
float D = (H * H - femur * femur - tibia * tibia) / (2 * femur * tibia); float D = (H * H - femur * femur - tibia * tibia) / (2 * femur * tibia);
float theta3 = acosf(max(-1.0f, min(1.0f, D))); float theta3 = acosf(std::max(-1.0f, std::min(1.0f, D)));
float theta2 = atan2f(z, G) - atan2f(tibia * sinf(theta3), femur + tibia * cosf(theta3)); float theta2 = atan2f(z, G) - atan2f(tibia * sinf(theta3), femur + tibia * cosf(theta3));
out[0] = RAD_TO_DEG_F(theta1); out[0] = RAD_TO_DEG_F(theta1);
out[1] = RAD_TO_DEG_F(theta2); out[1] = RAD_TO_DEG_F(theta2);