如何才能调试出C610+M2007组成伺服系统？

没有了解过基础之前，建议初学者先了解大疆C610+M2007的相关软硬件资料，再看看伺服系统的基本三环结构，当然stm32是需要熟练掌握的。
参考上述文章，再结合大疆官网给出的demo改动，才得以调试出 C610+M2007组成伺服系统用以参加四川省机器人大赛，并获得19年度一等奖。
好了，进入正题。
首先是通信，c610使用can 总线通信，因此需要对STM32的CAN初始化：
Can2Init();
由于我的系统中有4套电机，所以我过滤的时候保留这些电机的地址：


Can_filter_Structure.CAN_FilterIdHigh        = ((uint16_t)CAN_2006Moto1_ID << 5);
Can_filter_Structure.CAN_FilterIdLow        = ((uint16_t)CAN_2006Moto2_ID << 5);
Can_filter_Structure.CAN_FilterMaskIdHigh        = ((uint16_t)CAN_2006Moto3_ID << 5);
Can_filter_Structure.CAN_FilterMaskIdLow        = ((uint16_t)CAN_2006Moto4_ID << 5);


这是第一点要注意的，第二：初始化的顺序相当重要，STM32F407单片机CAN总线初始化需要注意一定的顺序，不然老是有can总线初始化失败或者串口工作不正常。如果使用STM32CUBE也许可以避免上述问题。我这里


初始化的顺序：
        uart6_init(115200);
        uart4_init(9600);
        uart3_init(9600);
        CAN1_Configuration();     //CAN13?¨º??¡¥
        Can2Init();   
        InitBar();   //舵机


先串口，再CAN1、CAN2，最后TIM


初始化过后，CAN2接收函数内部需要改动：
1、增加操作系统接口OSIntEnter()，OSIntExit();（无操作系统的无视此行）
2、针对地址做过滤，免得收到无效信息（CAN_2006Moto1_ID……）


moto_measure_t moto_chassis[4] = {0};//4 chassis moto
void CAN2_RX0_IRQHandler(void)
{
        CanRxMsg rx_message;
        u8 i;
        OSIntEnter();
       
//        extern moto_measure_t moto_chassis[4];


        if (CAN_GetITStatus(CAN2,CAN_IT_FMP0)!= RESET)
        {
                CAN_ClearITPendingBit(CAN2, CAN_IT_FMP0);
                CAN_Receive(CAN2, CAN_FIFO0, &rx_message);
               
                if((rx_message.IDE == CAN_Id_Standard)&&(rx_message.IDE == CAN_RTR_Data)&&(rx_message.DLC == 8)) //标准帧、数据帧、数据长度为8
                {
                        if((rx_message.StdId == CAN_2006Moto1_ID)||(rx_message.StdId == CAN_2006Moto2_ID)
                         ||(rx_message.StdId == CAN_2006Moto3_ID)||(rx_message.StdId == CAN_2006Moto4_ID))
                        {
                                i = rx_message.StdId - CAN_2006Moto1_ID;
                    get_moto_measure(&moto_chassis, &rx_message);
                        }
                }
        }
        OSIntExit();
}


然后就是重点了：


void TurnMoterSpeedPosCircleTask(void)
{
        int i;
        OS_ERR err;
        extern int Display1;
//        Can2Init();
        for(i=0; i   {       
                /*pid, maxout, intergral_limit, deadband, kp, ki, kd*/
    pid_param_init(&SpeedTurnMotorPid, 10000, 0, 100, 2.5, 0, 2.0);
          pid_param_init(&LocationTurnMotorPid, 10000, 0, 20, 0.3, 0.01, 0.35);
  }


        while(1)       
        {
                for(i=0; i                 {       
                        pid_calculate(&LocationTurnMotorPid, moto_chassis.total_angle, TurnMotorPos);
                        pid_calculate(&SpeedTurnMotorPid, moto_chassis.speed_rpm, LocationTurnMotorPid.output);       
               
                }
#ifndef MotorDebug
                set_moto_current(SpeedTurnMotorPid[0].output, SpeedTurnMotorPid[1].output,
                           SpeedTurnMotorPid[2].output, SpeedTurnMotorPid[3].output);
#endif
                OSTimeDlyHMSM(0,0,0,10,OS_OPT_TIME_HMSM_NON_STRICT,&err);
        }
}


上述代码使用pid控制器构建了速度环和位置环。


pid_param_init(&SpeedTurnMotorPid, 10000, 0, 100, 2.5, 0, 2.0); 这是对速度环的初始化，pid_param_init(&LocationTurnMotorPid, 10000, 0, 20, 0.3, 0.01, 0.35);是对位置环路的初始化。


   void pid_param_init(
        PID_TypeDef * pid,
        uint16_t maxout,
        uint16_t intergral_limit,
        float deadband,
        float         kp,
        float         ki,
        float         kd)


设置的参数有：pid, maxout, intergral_limit, deadband, kp, ki, kd。pid指你需要设定的PID控制器，本质是一个结构体。maxout指最大输出量，也就是限制幅度。intergral_limit是抗饱和积分，该参数可以限制积分器的最大输出值，我没有用，直接设置0。deadband是死区，意思是误差绝对值如果小于该参数，PID输出0，防止系统产生抖动。然后就是kp, ki, kd三个参数。这是初始化。
接下来就是PID环路的运算，我有四个电机，每个电机都要分别运算速度、位置环路，因此有个4循环：


  for(i=0; i                 {       
                        pid_calculate(&LocationTurnMotorPid, moto_chassis.total_angle, TurnMotorPos);
                        pid_calculate(&SpeedTurnMotorPid, moto_chassis.speed_rpm, LocationTurnMotorPid.output);       
                }


循环内部的PID运算环路：
float pid_calculate(PID_TypeDef* pid, float measure, float set)
参数意义分别是：PID控制器，分别对应速度环、位置环。measure是测量值，也就是通过编码器获取得到的速度值或者位置值。set是设定值，也就是期望达到的值。该函数运算后，将控制量（输出量）存放在结构体PID_TypeDef* pid当中，以供后续调用使用。
需要指出的是：位置环的设定值是指定值（例如：需要转到xx度），而速度环的设定值是位置环的输出值：LocationTurnMotorPid.output，而速度环路的输出值，直接给电流环作为输入使用：


set_moto_current(SpeedTurnMotorPid[0].output, SpeedTurnMotorPid[1].output,
                           SpeedTurnMotorPid[2].output, SpeedTurnMotorPid[3].output);