67 lines
2.4 KiB
C
67 lines
2.4 KiB
C
#include <stdio.h>
|
|
#include <math.h>
|
|
#include "User/component/vmc.h"
|
|
|
|
int main() {
|
|
// 测试VMC计算
|
|
VMC_t vmc;
|
|
VMC_Param_t param = {
|
|
.hip_length = 0.1f, // 10cm髋关节间距
|
|
.leg_1 = 0.15f, // 15cm大腿前端
|
|
.leg_2 = 0.15f, // 15cm大腿后端
|
|
.leg_3 = 0.15f, // 15cm小腿
|
|
.leg_4 = 0.15f, // 15cm小腿前端
|
|
.wheel_radius = 0.05f, // 5cm轮子半径
|
|
.wheel_mass = 1.0f // 1kg轮子质量
|
|
};
|
|
|
|
if (VMC_Init(&vmc, ¶m, 1000.0f) != 0) {
|
|
printf("VMC初始化失败\n");
|
|
return -1;
|
|
}
|
|
|
|
printf("测试VMC腿长计算\n");
|
|
printf("髋关节间距: %.3f\n", param.hip_length);
|
|
printf("腿段长度: L1=%.3f, L2=%.3f, L3=%.3f, L4=%.3f\n",
|
|
param.leg_1, param.leg_2, param.leg_3, param.leg_4);
|
|
printf("\n");
|
|
|
|
// 测试不同的关节角度配置
|
|
float test_angles[][2] = {
|
|
{0.0f, 0.0f}, // 水平伸展
|
|
{0.5f, -0.5f}, // 轻微弯曲
|
|
{1.0f, -1.0f}, // 中等弯曲
|
|
{1.5f, -1.5f}, // 较大弯曲
|
|
{-0.5f, 0.5f}, // 反向弯曲
|
|
};
|
|
|
|
int num_tests = sizeof(test_angles) / sizeof(test_angles[0]);
|
|
|
|
for (int i = 0; i < num_tests; i++) {
|
|
float phi1 = test_angles[i][0];
|
|
float phi4 = test_angles[i][1];
|
|
float body_pitch = 0.0f;
|
|
float body_pitch_rate = 0.0f;
|
|
|
|
if (VMC_ForwardSolve(&vmc, phi1, phi4, body_pitch, body_pitch_rate) == 0) {
|
|
float length, angle, d_length, d_angle;
|
|
VMC_GetVirtualLegState(&vmc, &length, &angle, &d_length, &d_angle);
|
|
|
|
float foot_x, foot_y;
|
|
VMC_GetFootPosition(&vmc, &foot_x, &foot_y);
|
|
|
|
printf("测试 %d: phi1=%.2f, phi4=%.2f\n", i+1, phi1, phi4);
|
|
printf(" 足端位置: (%.3f, %.3f)\n", foot_x, foot_y);
|
|
printf(" 虚拟腿长: %.3f\n", length);
|
|
printf(" 虚拟摆角: %.3f (%.1f度)\n", angle, angle * 180.0f / M_PI);
|
|
printf(" 内部变量: XC=%.3f, YC=%.3f, phi2=%.3f, phi3=%.3f\n",
|
|
vmc.leg.XC, vmc.leg.YC, vmc.leg.phi2, vmc.leg.phi3);
|
|
printf("\n");
|
|
} else {
|
|
printf("测试 %d: 求解失败 (phi1=%.2f, phi4=%.2f)\n", i+1, phi1, phi4);
|
|
}
|
|
}
|
|
|
|
return 0;
|
|
}
|