麦克纳姆轮运动分解

麦克纳姆轮运动分解，比CSDN更加快速上手

/************************** Dongguan-University of Technology -ACE**************************
 * @brief 底盘独立模式，右手系，食指X ，中指Y
 *
 * @param C 
 * @param X_IN
 * @param Y_IN
 * @param Z_IN
 * @param ExpRescue 
 ************************** Dongguan-University of Technology -ACE***************************/
// 小轮法向量向中心
// 1,2
// 3,4
void Chassis_Indepen_Drive(C_t *C, float X_IN, float Y_IN, float Z_IN, int16_t ExpRescue) {

    uint8_t i = 0;
    int16_t Val[4] = {0, 0, 0, 0}; //记录电机实时速度�?
    int16_t MAX = 0;               //四个电机里的�?大�?�度
    float SPID_OUT[4];             // PIDout

    if (Z_IN != 0)
        Z_IN = -Z_IN;

    /*底盘麦轮运动分解*/
    C->WheelMotor[0].ExpSpeed = -(X_IN + Y_IN - Z_IN);
    C->WheelMotor[1].ExpSpeed = (X_IN - Y_IN + Z_IN);
    C->WheelMotor[2].ExpSpeed = -(X_IN - Y_IN - Z_IN);
    C->WheelMotor[3].ExpSpeed = (X_IN + Y_IN + Z_IN);

    /*速度增益*/
    C->WheelMotor[0].ExpSpeed *= 10;
    C->WheelMotor[1].ExpSpeed *= 10;
    C->WheelMotor[2].ExpSpeed *= 10;
    C->WheelMotor[3].ExpSpeed *= 10;

    /*PID处理*/
    for (i = 0; i < 4; i++) {
        PidCalculate(&C->WheelMotor[i].SPID, C->WheelMotor[i].ExpSpeed, C->WheelMotor[i].Encoder->Speed[1]); //得到输出�?
        //        Val[i] = C->WheelMotor[i].Encoder->Speed[1];    //记录电机实时速度�?
    }
    
    CAN1_C620_OR_C610_201_TO_204_SendMsg(C->WheelMotor[0].SPID.out, C->WheelMotor[1].SPID.out,
                                         C->WheelMotor[2].SPID.out, C->WheelMotor[3].SPID.out);

}

先假定这几个轮子和电机在数组里的编号，电调ID往编号里+1即可。

对电机转速作出正负区分

    C->WheelMotor[0].ExpSpeed = -(X_IN + Y_IN - Z_IN);
    C->WheelMotor[1].ExpSpeed = (X_IN - Y_IN + Z_IN);
    C->WheelMotor[2].ExpSpeed = -(X_IN - Y_IN - Z_IN);
    C->WheelMotor[3].ExpSpeed = (X_IN + Y_IN + Z_IN);

可以看到0和2号被加了个负，因为与1和3的转动方向不一致，取反即可。

警告

如果想设置不同的ID，比如说顺时针或者逆时针比较好记，那就改变他这个负号改变速度方向即可。

对输入量进行麦轮运动分解

就能得到上述公式