如下是麦克纳姆轮工作机制，使用全力矩控制，附pid
把每一个轮子拿出来都是一个承受三个方向上的力的单一物体，根据力矩平衡，四个不同方向上的力会形成四轮平衡，以移动
不懂的话没关系
后附控制代码
void Chassis_Ctrl(void)
{
f_ang=DOUBLE_ECHO.GM_UP_ECHO.yaw_curangle-ORIN_ANGLE;
cha_goal_angle=f_ang;
if(RC_CtrlData.rc.s2==2)cha_goal_angle+=800;
wv=(cha_goal_angle)*0.3;
r_vx=RC_CtrlData.rc.ch0;
r_vy=RC_CtrlData.rc.ch1;
vx=(r_vx*cos(f_ang*PI/180.f)-r_vy*sin(f_ang*PI/180.f)) / 220;
vy=(r_vy*cos(f_ang*PI/180.f)+r_vx*sin(f_ang*PI/180.f)) / 220;
vw2 = -(+vx - vy - wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
vw3 = -(+vx + vy - wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
vw4 = (+vx - vy + wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
vw1 = (+vx + vy + wv * MOTOR_TO_CENTER) / (WHEEL_RADIUS * 2 * PI) * REDUCE_RATIO * 60;
if(vw1>=1800)vw1=1800;
if(vw2>=1800)vw2=1800;
if(vw3>=1800)vw3=1800;
if(vw4>=1800)vw4=1800;
if(vw1<=-1800)vw1=-1800;
if(vw2<=-1800)vw2=-1800;
if(vw3<=-1800)vw3=-1800;
if(vw4<=-1800)vw4=-1800;
GUARD_CHASSIS_MOTOR.data.output_current1= pidProcess(&hpid3508, vw1, m3508.m1.speed);
GUARD_CHASSIS_MOTOR.data.output_current2 = pidProcess(&hpid3508, vw2, m3508.m2.speed);
GUARD_CHASSIS_MOTOR.data.output_current3 = pidProcess(&hpid3508, vw3, m3508.m3.speed);
GUARD_CHASSIS_MOTOR.data.output_current4 = pidProcess(&hpid3508, vw4, m3508.m4.speed);
//通过can来控制电机
ctrl.pack = M3508_PACK_1_4;
ctrl.setcur[0] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current1;
ctrl.setcur[1] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current2;
ctrl.setcur[2] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current3;
ctrl.setcur[3] = (int16_t)GUARD_CHASSIS_MOTOR.data.output_current4;
m3508SetCurrent(&ctrl, &CAN1Handle);
vw1=vw2=vw3=vw4=0;
control_on++;
}