请问STM32使用CAN总线怎么实现大疆M3508电机的速度闭环控制？

一. 绪论

本文接上一篇STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法，在本文中介绍电机的控制原理和使用CAN总线实现电机速度闭环的代码实操，最后实现电机的速度控制任务。
二. 电机速度环PID原理

电机的种类特别多，小型机器人（无人机、无人车）上普遍使用直流无刷电机，航模使用的一般是高速低扭矩，车模一般通过减速器低速实现高扭矩。一般电机与电机驱动器配套使用，用的话只需要知道如何供电和如何给控制信号就行了。本文使用大疆的M3508电机，使用CAN总线进行控制，长相如图所示。



CAN总线是汽车行业的应用标准，稍大型的电机很多是CAN总线控制的。CAN总线是一种现场总线，具有速度快、传输距离远、连接节点多的优点，具有错误检测、错误通知和错误恢复功能，使得其特别适合工业过程监控设备的互联。CAN总线的知识点很复杂，现在升级版的CAN-Open更是乖乖不得了，我啃了好几天才大致弄懂一些。大家主要要会用就行了。
回到我们的电机，众所周知伺服电机有位置伺服、速度伺服、力伺服，大家一定也听过电机的三闭环控制，也就是位置环——速度环——电流环，如图所示，根据控制目标的不同，从外环到内环分别对应电机的位置伺服、速度伺服与力伺服，也可以说三种模式：位置模式、速度模式、转矩模式。




上一篇我们讲过姿态环的角度、角速度串级PID，所以看到这里的位置环——速度环——电流环大家一定不陌生了，而且似乎发现了什么有趣的东西，是不是外环的控制变量和内环的控制变量依次成导数关系？（电流决定的就是力，也就是角加速度）。
电机一般要配合驱动控制器使用，有的驱动器很牛皮，里面做好了三种模式，控制器给位置信号、速度信号就行了。有的次一点，只做了速度伺服，有的更次，啥也没做。我们今天用的大疆M33508电机就啥也没做，，，驱动器只是上报电机的速度、电流、转角、温度等信息，需要控制器去读这些信息，然后做闭环计算，控制器将计算结果传输给驱动器，再去控制电机电流。这里我们主要是做速度伺服。上图变成这样。






三. STM32使用CAN总线实现大疆M3508电机的速度闭环控制
这里我们需要对电机做一个速度闭环控制，由于大疆电机提供了例程，所以我直接移植了它的代码，当然有些地方做了修改。


创建一个motor_pid.h和motor_pid.c文件（其实完全可以用上一章写的pid.c和pid.h文件，这里因为是官方提供的例程，所以直接用了）。


motor_pid.h文件：


#ifndef __MOTOR_PID_H
#define __MOTOR_PID_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f7xx_hal.h"


enum
{
    LLAST = 0,
    LAST = 1,
    NOW = 2,


    POSITION_PID,
    DELTA_PID,
};
typedef struct __pid_t
{
    float p;
    float i;
    float d;


    float set[3]; //目标值,包含NOW， LAST， LLAST上上次
    float get[3]; //测量值
    float err[3]; //误差


    float pout; //p输出
    float iout; //i输出
    float dout; //d输出


    float pos_out;      //本次位置式输出
    float last_pos_out; //上次输出
    float delta_u;      //本次增量值
    float delta_out;    //本次增量式输出 = last_delta_out + delta_u
    float last_delta_out;


    float max_err;
    float deadband; //err < deadband return
    uint32_t pid_mode;
    uint32_t MaxOutput;     //输出限幅
    uint32_t IntegralLimit; //积分限幅


    void (*f_param_init)(struct __pid_t *pid, //PID参数初始化
                         uint32_t pid_mode,
                         uint32_t maxOutput,
                         uint32_t integralLimit,
                         float p,
                         float i,
                         float d);
    void (*f_pid_reset)(struct __pid_t *pid, float p, float i, float d); //pid三个参数修改


} pid_t;


void PID_struct_init(
    pid_t *pid,
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,


    float kp,
    float ki,
    float kd);


float pid_calc(pid_t *pid, float fdb, float ref);


#endif
motor_pid.c文件：


/* Includes ------------------------------------------------------------------*/
#include "motor_pid.h"
#include "mytype.h"
#include
//#include "cmsis_os.h"


#define ABS(x)                ((x>0)? (x): (-x))


void abs_limit(float *a, float ABS_MAX){
    if(*a > ABS_MAX)
        *a = ABS_MAX;
    if(*a < -ABS_MAX)
        *a = -ABS_MAX;
}
/*参数初始化--------------------------------------------------------------*/
static void pid_param_init(
    pid_t *pid,
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,
    float         kp,
    float         ki,
    float         kd)
{
   
    pid->IntegralLimit = intergral_limit;
    pid->MaxOutput = maxout;
    pid->pid_mode = mode;
   
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
   
}
/*中途更改参数设定(调试)------------------------------------------------------------*/
static void pid_reset(pid_t        *pid, float kp, float ki, float kd)
{
    pid->p = kp;
    pid->i = ki;
    pid->d = kd;
}


/**
    *@bref. calculate delta PID and position PID
    *@param[in] set： target
    *@param[in] real        measure
    */
float pid_calc(pid_t* pid, float get, float set){
    pid->get[NOW] = get;
    pid->set[NOW] = set;
    pid->err[NOW] = set - get;        //set - measure
    if (pid->max_err != 0 && ABS(pid->err[NOW]) >  pid->max_err  )
                return 0;
        if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband)
                return 0;
   
    if(pid->pid_mode == POSITION_PID) //位置式p
    {
        pid->pout = pid->p * pid->err[NOW];
        pid->iout += pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST] );
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->pos_out = pid->pout + pid->iout + pid->dout;
        abs_limit(&(pid->pos_out), pid->MaxOutput);
        pid->last_pos_out = pid->pos_out;        //update last time
    }
    else if(pid->pid_mode == DELTA_PID)//增量式P
    {
        pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
        pid->iout = pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
        
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->delta_u = pid->pout + pid->iout + pid->dout;
        pid->delta_out = pid->last_delta_out + pid->delta_u;
        abs_limit(&(pid->delta_out), pid->MaxOutput);
        pid->last_delta_out = pid->delta_out;        //update last time
    }
   
    pid->err[LLAST] = pid->err[LAST];
    pid->err[LAST] = pid->err[NOW];
    pid->get[LLAST] = pid->get[LAST];
    pid->get[LAST] = pid->get[NOW];
    pid->set[LLAST] = pid->set[LAST];
    pid->set[LAST] = pid->set[NOW];
    return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//       
}


/**
    *@bref. special calculate position PID @attention @use @gyro data!!
    *@param[in] set： target
    *@param[in] real        measure
    */
float pid_sp_calc(pid_t* pid, float get, float set, float gyro){
    pid->get[NOW] = get;
    pid->set[NOW] = set;
    pid->err[NOW] = set - get;        //set - measure
   
   
    if(pid->pid_mode == POSITION_PID) //位置式p
    {
        pid->pout = pid->p * pid->err[NOW];
                                if(fabs(pid->i) >= 0.001f)
                                        pid->iout += pid->i * pid->err[NOW];
                                else
                                        pid->iout = 0;
        pid->dout = -pid->d * gyro/100.0f;       
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->pos_out = pid->pout + pid->iout + pid->dout;
        abs_limit(&(pid->pos_out), pid->MaxOutput);
        pid->last_pos_out = pid->pos_out;        //update last time
    }
    else if(pid->pid_mode == DELTA_PID)//增量式P
    {
       pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
       pid->iout = pid->i * pid->err[NOW];
       pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
      
       abs_limit(&(pid->iout), pid->IntegralLimit);
       pid->delta_u = pid->pout + pid->iout + pid->dout;
       pid->delta_out = pid->last_delta_out + pid->delta_u;
       abs_limit(&(pid->delta_out), pid->MaxOutput);
       pid->last_delta_out = pid->delta_out;        //update last time
    }
   
    pid->err[LLAST] = pid->err[LAST];
    pid->err[LAST] = pid->err[NOW];
    pid->get[LLAST] = pid->get[LAST];
    pid->get[LAST] = pid->get[NOW];
    pid->set[LLAST] = pid->set[LAST];
    pid->set[LAST] = pid->set[NOW];
    return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//       
}
/*pid总体初始化-----------------------------------------------------------------*/
void PID_struct_init(
    pid_t* pid,
    uint32_t mode,
    uint32_t maxout,
    uint32_t intergral_limit,
   
    float         kp,
    float         ki,
    float         kd)
{
    /*init function pointer*/
    pid->f_param_init = pid_param_init;
    pid->f_pid_reset = pid_reset;
//        pid->f_cal_pid = pid_calc;       
//        pid->f_cal_sp_pid = pid_sp_calc;        //addition
               
    /*init pid param */
    pid->f_param_init(pid, mode, maxout, intergral_limit, kp, ki, kd);
       
}
再创建一个motor.h和motor.c文件，用于写电机PID控制的应用代码。


motor.h内容为：


#ifndef __MOTOR
#define __MOTOR


#include "stm32f7xx_hal.h"
#include "mytype.h"
#include "can.h"


/*电机的参数结构体*/
typedef struct{
        int16_t                 speed_rpm;
    int16_t          real_current;
    int16_t          given_current;
    uint8_t          hall;
        uint16_t         angle;                                //abs angle range:[0,8191]
        uint16_t         last_angle;        //abs angle range:[0,8191]
        uint16_t        offset_angle;
        int32_t                round_cnt;
        int32_t                total_angle;
        u8                        buf_idx;
        u16                        angle_buf[FILTER_BUF_LEN];
        u16                        fited_angle;
        u32                        msg_cnt;
}moto_measure_t;


#define motor_num 6                //电机数量


/* Extern  ------------------------------------------------------------------*/
extern moto_measure_t  moto_chassis[];


u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4);
#endif
motor.c文件：


#include "can.h"
#include "motor.h"
//#include "cmsis_os.h"


moto_measure_t moto_chassis[motor_num] = {0};                //4 chassis moto
moto_measure_t moto_info;


void get_total_angle(moto_measure_t *p);
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);


/*******************************************************************************************
  * @Func                        void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
  * @Brief    接收通过CAN发过来的信息
  * @Param               
  * @Retval                None
  * @Date     2015/11/24
*******************************************************************************************/
u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
        if(HAL_CAN_Receive(hcan,CAN_FIFO0,0)!=HAL_OK) return 0;//接收数据       


        ptr->last_angle = ptr->angle;
        ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
        ptr->real_current  = (int16_t)(hcan->pRxMsg->Data[2]<<8 | hcan->pRxMsg->Data[3]);
        ptr->speed_rpm = ptr->real_current;        //这里是因为两种电调对应位不一样的信息
        ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 | hcan->pRxMsg->Data[5])/-5;
        ptr->hall = hcan->pRxMsg->Data[6];
        if(ptr->angle - ptr->last_angle > 4096)
                ptr->round_cnt --;
        else if (ptr->angle - ptr->last_angle < -4096)
                ptr->round_cnt ++;
        ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
        return hcan->pRxMsg->DLC;
}


/*this function should be called after system+can init */
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
        ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
        ptr->offset_angle = ptr->angle;
}


#define ABS(x)        ( (x>0) ? (x) : (-x) )
/**
*@bref 电机上电角度=0， 之后用这个函数更新3510电机的相对开机后（为0）的相对角度。
        */
void get_total_angle(moto_measure_t *p){
       
        int res1, res2, delta;
        if(p->angle < p->last_angle){                        //可能的情况
                res1 = p->angle + 8192 - p->last_angle;        //正转，delta=+
                res2 = p->angle - p->last_angle;                                //反转        delta=-
        }else{        //angle > last
                res1 = p->angle - 8192 - p->last_angle ;//反转        delta -
                res2 = p->angle - p->last_angle;                                //正转        delta +
        }
        //不管正反转，肯定是转的角度小的那个是真的
        if(ABS(res1)                 delta = res1;
        else
                delta = res2;


        p->total_angle += delta;
        p->last_angle = p->angle;
}


u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4)
{
        hcan->pTxMsg->StdId = SID;
        hcan->pTxMsg->IDE = CAN_ID_STD;
        hcan->pTxMsg->RTR = CAN_RTR_DATA;
        hcan->pTxMsg->DLC = 0x08;
        hcan->pTxMsg->Data[0] = iq1 >> 8;
        hcan->pTxMsg->Data[1] = iq1;
        hcan->pTxMsg->Data[2] = iq2 >> 8;
        hcan->pTxMsg->Data[3] = iq2;
        hcan->pTxMsg->Data[4] = iq3 >> 8;
        hcan->pTxMsg->Data[5] = iq3;
        hcan->pTxMsg->Data[6] = iq4 >> 8;
        hcan->pTxMsg->Data[7] = iq4;
       
        //HAL_CAN_Transmit(hcan, 1000);


        if(HAL_CAN_Transmit(&CAN1_Handler,1000)!=HAL_OK) return 1;     //发送
    return 0;       
}       
motor.c里面主要实现两个函数，一个是get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)，用于获得各个电机的当前信息，一个是set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4)，用来经过PID计算后，设置电机电流（速度PID输出的是电流）。


四. UCOS-III电机控制任务的实现
写完了驱动函数和初级应用函数，下面可以写核心代码了，在上一章STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法的基础上，补充完整motor_task.


//motor任务
u8 motor_task(void *p_arg)
{
        OS_ERR err;
        CPU_SR_ALLOC();


        u8 i;
        u16 retry;
        u8 flag_motor[motor_num];                //电机信息接受成功标志
        pid_t pid_speed[motor_num];                //电机速度PID环
        float set_speed_temp;                        //加减速时的临时设定速度
        int16_t delta;                                        //设定速度与实际速度只差值
        int16_t max_speed_change = 400; //电机单次最大变化速度，加减速用
        //PID初始化
        for (i = 0; i < 4; i++)
        {
                PID_struct_init(&pid_speed, POSITION_PID, 16384, 16384,
                                                1.5f, 0.1f, 0.0f); //4 motos angular rate closeloop.
        }


        while (1)
        {
                //PID采样，获取电机数据
                retry = 0;
                flag_motor[0] = flag_motor[1] = flag_motor[2] = flag_motor[3] = 0;
                while (retry < 20)
                {
                        get_moto_measure(&moto_info, &CAN1_Handler);
                        if (CAN1_Handler.pRxMsg->StdId == 0x201)
                        {
                                moto_chassis[0] = moto_info;
                                flag_motor[0] = 1;
                        }
                        if (CAN1_Handler.pRxMsg->StdId == 0x202)
                        {
                                moto_chassis[1] = moto_info;
                                flag_motor[1] = 1;
                        }
                        if (CAN1_Handler.pRxMsg->StdId == 0x203)
                        {
                                moto_chassis[2] = moto_info;
                                flag_motor[2] = 1;
                        }
                        if (CAN1_Handler.pRxMsg->StdId == 0x204)
                        {
                                moto_chassis[3] = moto_info;
                                flag_motor[3] = 1;
                        }
                       
                        if (flag_motor[0] && flag_motor[2]&& flag_motor[1]&& flag_motor[3])
                                break;
                        else
                                retry++;
                }


                //PID计算输出


                //OS_CRITICAL_ENTER();
                for (i = 0; i < 5; i++)
                {
                        //PID计算


                        //加减速
                        delta = (int16_t)set_speed - moto_chassis.speed_rpm;
                        if (delta > max_speed_change)
                                set_speed_temp = (float)(moto_chassis.speed_rpm + max_speed_change);
                        else if (delta < -max_speed_change)
                                set_speed_temp = (float)(moto_chassis.speed_rpm - max_speed_change);
                        else
                                set_speed_temp = set_speed;
                        //pid_calc(&pid_speed, (float)moto_chassis.speed_rpm, set_speed);
                        pid_calc(&pid_speed, (float)moto_chassis.speed_rpm, set_speed_temp);
                }


//                printf("PID out: %frn", pid_speed[1].pos_out);
//                printf("real speed: %d rn", moto_chassis[1].speed_rpm);
//                printf("set speed: %f rn", set_speed[1]);
                //PID 输出


                set_moto_current(&CAN1_Handler, 0x200, (s16)(pid_speed[0].pos_out),
                                                 (s16)(pid_speed[1].pos_out),
                                                 (s16)(pid_speed[2].pos_out),
                                                 (s16)(pid_speed[3].pos_out));


                //OS_CRITICAL_EXIT();
                delay_ms(5);
        }
}


电机任务首先进行PID的初始化，PID_struct_init(&pid_speed, POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f)。初始化四个电机为位置式PID、PID输出限幅16384，PID参数为Kp=1.5， Ki=0.1, Kd=0。


之后进入while（1）循环，查询电机当前速度（get_moto_measure），然后依次对四个电机进行PID计算(pid_calc)，然后一次性将四个电机的PID计算结果输送到CAN总线(set_moto_current)。这里我还做了加减速处理，避免电机速度调整太快超过PID的输出限幅。


到此完整的链路就实现了。建议大家再把前面的四篇文章读一下，对于整个控制逻辑和数据传输链路就会更清楚了。整体来说四个任务是一环套一环的，但是又是独立的。


这样的代码架构在程序设计与代码调试上会非常快，能够快速定位Bug，所以建议还在裸奔的小伙伴们去学一学操作系统，会大大提升程序设计水平。