diff --git a/esp32/lib/ESP32-sveltekit/Kinematics.h b/esp32/lib/ESP32-sveltekit/Kinematics.h index d8701de..5c838e2 100644 --- a/esp32/lib/ESP32-sveltekit/Kinematics.h +++ b/esp32/lib/ESP32-sveltekit/Kinematics.h @@ -1,30 +1,21 @@ #ifndef Kinematics_h #define Kinematics_h -#include -#include +#include -#define RAD2DEG 57.295779513082321 // 180 / PI -#define DEG2RAD 0.017453292519943 struct body_state_t { float omega, phi, psi, xm, ym, zm; float feet[4][4]; - void updateFeet(const float newFeet[4][4]) { - for (int i = 0; i < 4; ++i) { - for (int j = 0; j < 4; ++j) { - feet[i][j] = newFeet[i][j]; - } - } - } + void updateFeet(const float newFeet[4][4]) { COPY_2D_ARRAY_4x4(feet, newFeet); } bool isEqual(const body_state_t &other) const { if (omega != other.omega || phi != other.phi || psi != other.psi || xm != other.xm || ym != other.ym || zm != other.zm) { return false; } - return memcmp(feet, other.feet, sizeof(feet)) == 0; + return ARRAY_EQUAL(feet, other.feet); } }; @@ -48,8 +39,8 @@ class Kinematics { float point[4]; float Q1[4][4]; - const float sHp = sin(PI / 2); - const float cHp = cos(PI / 2); + const float sHp = sinf(PI_F / 2); + const float cHp = cosf(PI_F / 2); float point_lf[4][4]; @@ -79,33 +70,33 @@ class Kinematics { currentState = body_state; ret += inverse(Tlf, inv); - dspm_mult_f32_ae32((float *)inv, (float *)body_state.feet[0], (float *)point, 4, 4, 1); + MAT_MULT(inv, body_state.feet[0], point, 4, 4, 1); legIK((float *)point, result); ret += inverse(Trf, inv); - dspm_mult_f32_ae32((float *)Ix, (float *)inv, (float *)Q1, 4, 4, 4); - dspm_mult_f32_ae32((float *)Q1, (float *)body_state.feet[1], (float *)point, 4, 4, 1); + MAT_MULT(Ix, inv, Q1, 4, 4, 4); + MAT_MULT(Q1, body_state.feet[1], point, 4, 4, 1); legIK((float *)point, result + 3); ret += inverse(Tlb, inv); - dspm_mult_f32_ae32((float *)inv, (float *)body_state.feet[2], (float *)point, 4, 4, 1); + MAT_MULT(inv, body_state.feet[2], point, 4, 4, 1); legIK((float *)point, result + 6); ret += inverse(Trb, inv); - dspm_mult_f32_ae32((float *)Ix, (float *)inv, (float *)Q1, 4, 4, 4); - dspm_mult_f32_ae32((float *)Q1, (float *)body_state.feet[3], (float *)point, 4, 4, 1); + MAT_MULT(Ix, inv, Q1, 4, 4, 4); + MAT_MULT(Q1, body_state.feet[3], point, 4, 4, 1); legIK((float *)point, result + 9); return ret; } esp_err_t bodyIK(const body_state_t p) { - float cos_omega = cos(p.omega * DEG2RAD); - float sin_omega = sin(p.omega * DEG2RAD); - float cos_phi = cos(p.phi * DEG2RAD); - float sin_phi = sin(p.phi * DEG2RAD); - float cos_psi = cos(p.psi * DEG2RAD); - float sin_psi = sin(p.psi * DEG2RAD); + float cos_omega = COS_DEG_F(p.omega); + float sin_omega = SIN_DEG_F(p.omega); + float cos_phi = COS_DEG_F(p.phi); + float sin_phi = SIN_DEG_F(p.phi); + float cos_psi = COS_DEG_F(p.psi); + float sin_psi = SIN_DEG_F(p.psi); float Tm[4][4] = {{cos_phi * cos_psi, -sin_psi * cos_phi, sin_phi, p.xm}, {sin_omega * sin_phi * cos_psi + sin_psi * cos_omega, @@ -122,28 +113,28 @@ class Kinematics { float point_rb[4][4] = {{cHp, 0, sHp, -L / 2}, {0, 1, 0, 0}, {-sHp, 0, cHp, -W / 2}, {0, 0, 0, 1}}; - dspm_mult_f32_ae32((float *)Tm, (float *)point_lf, (float *)Tlf, 4, 4, 4); - dspm_mult_f32_ae32((float *)Tm, (float *)point_rf, (float *)Trf, 4, 4, 4); - dspm_mult_f32_ae32((float *)Tm, (float *)point_lb, (float *)Tlb, 4, 4, 4); - dspm_mult_f32_ae32((float *)Tm, (float *)point_rb, (float *)Trb, 4, 4, 4); + MAT_MULT(Tm, point_lf, Tlf, 4, 4, 4); + MAT_MULT(Tm, point_rf, Trf, 4, 4, 4); + MAT_MULT(Tm, point_lb, Tlb, 4, 4, 4); + MAT_MULT(Tm, point_rb, Trb, 4, 4, 4); return ESP_OK; } void legIK(float point[4], float result[3]) { float x = point[0], y = point[1], z = point[2]; - float F = sqrt(x * x + y * y - l1 * l1); - F = isnan(F) ? l1 : F; + float F = sqrtf(x * x + y * y - l1 * l1); + F = isnanf(F) ? l1 : F; float G = F - l2; - float H = sqrt(G * G + z * z); + float H = sqrtf(G * G + z * z); - float theta1 = -atan2(y, x) - atan2(F, -l1); - float theta3 = acos((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4)); + float theta1 = -atan2f(y, x) - atan2f(F, -l1); + float theta3 = acosf((H * H - l3 * l3 - l4 * l4) / (2 * l3 * l4)); if (isnan(theta3)) theta3 = 0; - float theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3)); - result[0] = theta1 * RAD2DEG; - result[1] = theta2 * RAD2DEG; - result[2] = theta3 * RAD2DEG; + float theta2 = atan2f(z, G) - atan2f(l4 * sinf(theta3), l3 + l4 * cosf(theta3)); + result[0] = RAD_TO_DEG_F(theta1); + result[1] = RAD_TO_DEG_F(theta2); + result[2] = RAD_TO_DEG_F(theta3); } esp_err_t inverse(float a[4][4], float b[4][4]) { diff --git a/esp32/lib/ESP32-sveltekit/MathUtils.h b/esp32/lib/ESP32-sveltekit/MathUtils.h index 9c89525..1011dad 100644 --- a/esp32/lib/ESP32-sveltekit/MathUtils.h +++ b/esp32/lib/ESP32-sveltekit/MathUtils.h @@ -1,8 +1,55 @@ #ifndef MATHUTILS_H #define MATHUTILS_H +#include #include +#define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0])) + +#define ARRAY_EQUAL(arr1, arr2) (memcmp((arr1), (arr2), sizeof(arr1)) == 0) + +#define COPY_2D_ARRAY_4x4(dest, src) \ + do { \ + (dest)[0][0] = (src)[0][0]; \ + (dest)[0][1] = (src)[0][1]; \ + (dest)[0][2] = (src)[0][2]; \ + (dest)[0][3] = (src)[0][3]; \ + (dest)[1][0] = (src)[1][0]; \ + (dest)[1][1] = (src)[1][1]; \ + (dest)[1][2] = (src)[1][2]; \ + (dest)[1][3] = (src)[1][3]; \ + (dest)[2][0] = (src)[2][0]; \ + (dest)[2][1] = (src)[2][1]; \ + (dest)[2][2] = (src)[2][2]; \ + (dest)[2][3] = (src)[2][3]; \ + (dest)[3][0] = (src)[3][0]; \ + (dest)[3][1] = (src)[3][1]; \ + (dest)[3][2] = (src)[3][2]; \ + (dest)[3][3] = (src)[3][3]; \ + } while (0) + +#define MAT_MULT(A, B, result, rows, cols, result_cols) \ + dspm_mult_f32_ae32((float *)(A), (float *)(B), (float *)(result), (rows), (cols), (result_cols)) + +#define INT_TO_STRING(state, output) \ + do { \ + itoa((int)(state), (output), 10); \ + } while (0) + +#define PI_F 3.1415927f + +#define DEG2RAD_F 0.0174532f + +#define RAD2DEG_F 57.2957795f + +#define RAD_TO_DEG_F(rad) ((rad) * RAD2DEG_F) + +#define DEG_TO_RAD_F(deg) ((deg) * DEG2RAD_F) + +#define COS_DEG_F(deg) (cosf(DEG_TO_RAD_F(deg))) + +#define SIN_DEG_F(deg) (sinf(DEG_TO_RAD_F(deg))) + inline float lerp(float start, float end, float t) { return (1 - t) * start + t * end; } inline bool isEqual(float a, float b, float epsilon) { return std::fabs(a - b) < epsilon; }