大疆M3508电机使用CAN通信进行速度PID闭环控制详解

最新推荐文章于 2023-09-17 19:54:45 发布

何为其然

最新推荐文章于 2023-09-17 19:54:45 发布

阅读量5.3w

收藏 1k

点赞数 167

分类专栏：运维 STM32 嵌入式文章标签：嵌入式 stm32 算法

本文链接：https://blog.csdn.net/qq_30267617/article/details/119651826

版权

嵌入式同时被 3 个专栏收录

31 篇文章 95 订阅

订阅专栏

STM32

26 篇文章 30 订阅

订阅专栏

运维

10 篇文章 9 订阅

订阅专栏

一. 简介

之前写过一篇文章STM32实现四驱小车（五）电机控制任务——电机速度PID控制算法，其中是以大疆的M3508电机为例进行讲解的（没错，就是RoboMaster机器人同款电机，不过Robomaster上的电机好像是小一号的M2006）。不少小伙伴私信问我要代码，我都回复说不是有官方demo么。后来问的人多了我大概明白了，看来官方的Demo还是有点门槛。可能是带FreeRTOS操作系统看不懂，或者对CAN通信一知半解，或者电机PID控制理解不了，或者代码框架不太能跟上。于是决定单独写一篇专门讲解大疆M3508电机使用CAN通信进行速度闭环控制的文章。

本文分别使用CAN查询接收与CAN中断接收两种方式改写了官方Demo，两者都去掉了操作系统，使代码精简易读。

本文环境：

Keil MDK5.14
STM32CubeMX6.2.1
开发板/芯片：正点原子探索者F407/正点原子阿波罗F767IGT6

实现功能：

大疆M3508速度PID控制
说明：CAN查询接收与中断接收两种方式，无操作系统

下载链接：

大疆M3508电机CAN通信速度PID控制（查询与中断方式-无操作系统）

二. 电机通信协议

关于电机的说明请参照电机配套的C620电调说明书，这里简单介绍一下。C620电调可以通过两种方式控制电机，一种是接收PWM信号，这种方式接线与控制都比较简单，直接使用PWM进行速度控制（已经是闭环），另一种方式就是CAN通信，这种方式电调定期上传电机的电流、速度、位置等信息，控制器读取CAN报文之后通过PID计算得到输出量，再通过CAN总线下发报文，从而实现速度PID控制。虽然PWM方式简单易行，但经实际测试效果并不好，速度稳定性不行。

CAN上传报文：

CAN上传报文是电调发给单片机的，上传电机数据。

标识符：0×200+电调ID (如：ID为1,该标识符为0×201)
帧类型：标准帧
帧格式：DATA
DLC:8字节
在这里插入图片描述
发送频率:1KHz(默认值,可在 RoboMaster Assistant软件中修改发送频率)
转子机械角度值范围:0~ 8191(对应转子机械角度为0~360°)
转子转速值的单位为:RPM
电机温度的单位为:℃

可以看到每个电调以1KHz的频率上报电机数据，每帧数据包含电机的位置、转速、转矩电流、温度四个信息量，我们最关注的就是转速，通过上报的转速与设定的速度比较，进行PID控制。

CAN下发报文：

CAN下发报文是单片机发给电调的，设置电调的电流输出

两个标识符(0200和0x1FF)各自对应控制4个电调。控制电流值范围-16384~ 0~ 16384,对应电调输出的转矩电流范围-20~ 0~ 20A。

标识符: 0x200 帧格式:DATA 帧类型: 标准帧 DLC:8字节
在这里插入图片描述
标识符: 0x1FF 帧格式:DATA 帧类型: 标准帧 DLC:8字节

下发的报文只包含电流值信息，这是PID计算的输出结果，至于为什么是电流后面会说，每帧数据控制四个电机，最多通过两组不同ID的报文控制8个电机。

其他的设置电调ID的方法以及校准、指示灯等请阅读说明书。

三. 电机PID控制原理

电机是一种通电就会转的东东，正负极反接就会反转，电压增大转速就会增大，但是我们希望能够精准控制速度，甚至控制它的位置，所以需要有电机驱动器。最低级的电机驱动器只提供功率放大或整流，PID计算需要通过控制器计算；一些高级点的驱动器内部做了PID控制，控制器只需要给速度指令或位置指令就行了，这就是控制的分层。我们使用的M3508电机配套的C620电调，在用PWM控制时它是自带速度PID的，使用CAN总线控制时它就不带PID，需要单片机来计算。

伺服电机有位置伺服、速度伺服、力伺服，大家可能也听过电机的三闭环控制，也就是位置环——速度环——电流环，如图所示，根据控制目标的不同，从外环到内环分别对应电机的位置伺服、速度伺服与力伺服，也可以说三种模式：位置模式、速度模式、转矩模式。

在这里插入图片描述我们的目标是控制速度，上图变成下面这样：

在这里插入图片描述加减速处理的目的是防止偏差值太大，导致速度环PID输出结果太大，超过电机的响应范围，结果可能导致死机、断续旋转。3508就有这个问题的，加入加减速就是让电机速度变化更加平缓。

由于速度换输出的是电流值，所以这也解释了为什么单片机通过CAN报文下发的是转矩电流。至于电流环在哪？那是硬件决定的。

四. 官方代码移植-中断接收

好啦，通信协议和PID原理都搞通了，下面开始写代码。官方提供的例程写的真心很好的，虽然带了个FreeRTOS操作系统，但实际上任务太简单单一，操作系统根本没用上。我们移植它。官方的demo是STM32F4的，这里还是使用STM32F4，实际上我自己用的STM32F7，由于F4和F7都是M4内核，实际上代码是几乎一样的。而使用STM32F1需要注意，F1使用的M3内核，很多寄存器都是不一样的，HAL库就有很大差异，所以驱动部分需要改写。

1. CAN初始化：

创建can.h文件：

#ifndef __CAN_H
#define __CAN_H
#include "sys.h"

//CAN1接收RX0中断使能
#define CAN1_RX0_INT_ENABLE	1		//0,不使能;1,使能.

u8 CAN1_Mode_Init(u32 tsjw,u32 tbs2,u32 tbs1,u16 brp,u32 mode);//CAN初始化
u8 CAN1_Send_Msg(u8* msg,u8 len);						//发送数据
u8 CAN1_Receive_Msg(u8 *buf);							//接收数据

extern CAN_HandleTypeDef   CAN1_Handler;   //CAN1句柄
#endif

这里面有一个宏定义CAN1_RX0_INT_ENABLE，定义是否使用CAN接收中断，这里我们设为1，使用它。

创建can.c文件：

#include "can.h"
#include "usart.h"
#include "delay.h"
#include "led.h"

CAN_HandleTypeDef CAN1_Handler; //CAN1句柄
CanTxMsgTypeDef TxMessage;      //发送消息
CanRxMsgTypeDef RxMessage;      //接收消息

CAN初始化
//tsjw:重新同步跳跃时间单元.范围:CAN_SJW_1TQ~CAN_SJW_4TQ
//tbs2:时间段2的时间单元.   范围:CAN_BS2_1TQ~CAN_BS2_8TQ;
//tbs1:时间段1的时间单元.   范围:CAN_BS1_1TQ~CAN_BS1_16TQ
//brp :波特率分频器.范围:1~1024; tq=(brp)*tpclk1
//波特率=Fpclk1/((tbs1+tbs2+1)*brp); 其中tbs1和tbs2我们只用关注标识符上标志的序号，例如CAN_BS2_1TQ，我们就认为tbs2=1来计算即可。
//mode:CAN_MODE_NORMAL,普通模式;CAN_MODE_LOOPBACK,回环模式;
//Fpclk1的时钟在初始化的时候设置为45M,如果设置CAN1_Mode_Init(CAN_SJW_1tq,CAN_BS2_6tq,CAN_BS1_8tq,6,CAN_MODE_LOOPBACK);
//则波特率为:45M/((6+8+1)*6)=500Kbps
//返回值:0,初始化OK;
//    其他,初始化失败;

u8 CAN1_Mode_Init(u32 tsjw, u32 tbs2, u32 tbs1, u16 brp, u32 mode)
{
    CAN_FilterConfTypeDef CAN1_FilerConf;

    CAN1_Handler.Instance = CAN1;
    CAN1_Handler.pTxMsg = &TxMessage;  //发送消息
    CAN1_Handler.pRxMsg = &RxMessage;  //接收消息
    CAN1_Handler.Init.Prescaler = brp; //分频系数(Fdiv)为brp+1
    CAN1_Handler.Init.Mode = mode;     //模式设置
    CAN1_Handler.Init.SJW = tsjw;      //重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1TQ~CAN_SJW_4TQ
    CAN1_Handler.Init.BS1 = tbs1;      //tbs1范围CAN_BS1_1TQ~CAN_BS1_16TQ
    CAN1_Handler.Init.BS2 = tbs2;      //tbs2范围CAN_BS2_1TQ~CAN_BS2_8TQ
    CAN1_Handler.Init.TTCM = DISABLE;  //非时间触发通信模式
    CAN1_Handler.Init.ABOM = DISABLE;  //软件自动离线管理
    CAN1_Handler.Init.AWUM = DISABLE;  //睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
    CAN1_Handler.Init.NART = ENABLE;   //禁止报文自动传送
    CAN1_Handler.Init.RFLM = DISABLE;  //报文不锁定,新的覆盖旧的
    CAN1_Handler.Init.TXFP = DISABLE;  //优先级由报文标识符决定

    if (HAL_CAN_Init(&CAN1_Handler) != HAL_OK)
        return 1; //初始化

    CAN1_FilerConf.FilterIdHigh = 0X0000; //32位ID
    CAN1_FilerConf.FilterIdLow = 0X0000;
    CAN1_FilerConf.FilterMaskIdHigh = 0X0000; //32位MASK
    CAN1_FilerConf.FilterMaskIdLow = 0X0000;
    CAN1_FilerConf.FilterFIFOAssignment = CAN_FILTER_FIFO0; //过滤器0关联到FIFO0
    CAN1_FilerConf.FilterNumber = 0;                        //过滤器0
    CAN1_FilerConf.FilterMode = CAN_FILTERMODE_IDMASK;
    CAN1_FilerConf.FilterScale = CAN_FILTERSCALE_32BIT;
    CAN1_FilerConf.FilterActivation = ENABLE; //激活滤波器0
    CAN1_FilerConf.BankNumber = 14;

    if (HAL_CAN_ConfigFilter(&CAN1_Handler, &CAN1_FilerConf) != HAL_OK)
        return 2; //滤波器初始化

    return 0;
}

//CAN底层驱动，引脚配置，时钟配置，中断配置
//此函数会被HAL_CAN_Init()调用
//hcan:CAN句柄
void HAL_CAN_MspInit(CAN_HandleTypeDef *hcan)
{
    GPIO_InitTypeDef GPIO_Initure;

    __HAL_RCC_CAN1_CLK_ENABLE();  //使能CAN1时钟
    __HAL_RCC_GPIOA_CLK_ENABLE(); //开启GPIOA时钟

    GPIO_Initure.Pin = GPIO_PIN_11 | GPIO_PIN_12; //PA11,12
    GPIO_Initure.Mode = GPIO_MODE_AF_PP;          //推挽复用
    GPIO_Initure.Pull = GPIO_PULLUP;              //上拉
    GPIO_Initure.Speed = GPIO_SPEED_FAST;         //快速
    GPIO_Initure.Alternate = GPIO_AF9_CAN1;       //复用为CAN1
    HAL_GPIO_Init(GPIOA, &GPIO_Initure);          //初始化

#if CAN1_RX0_INT_ENABLE
    __HAL_CAN_ENABLE_IT(&CAN1_Handler, CAN_IT_FMP0); //FIFO0消息挂起中断允许.
    //CAN1->IER|=1<<1;		//FIFO0消息挂起中断允许.
    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 1, 2); //抢占优先级1，子优先级2
    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);         //使能中断
#endif
}

#if CAN1_RX0_INT_ENABLE //使能RX0中断
//CAN中断服务函数
void CAN1_RX0_IRQHandler(void)
{
    HAL_CAN_IRQHandler(&CAN1_Handler); //此函数会调用CAN_Receive_IT()接收数据
}
#endif

//can发送一组数据(固定格式:ID为0X12,标准帧,数据帧)
//len:数据长度(最大为8)
//msg:数据指针,最大为8个字节.
//返回值:0,成功;
//		 其他,失败;
u8 CAN1_Send_Msg(u8 *msg, u8 len)
{
    u16 i = 0;
    CAN1_Handler.pTxMsg->StdId = 0X12;       //标准标识符
    CAN1_Handler.pTxMsg->ExtId = 0x12;       //扩展标识符(29位)
    CAN1_Handler.pTxMsg->IDE = CAN_ID_STD;   //使用标准帧
    CAN1_Handler.pTxMsg->RTR = CAN_RTR_DATA; //数据帧
    CAN1_Handler.pTxMsg->DLC = len;
    for (i = 0; i < len; i++)
        CAN1_Handler.pTxMsg->Data[i] = msg[i];
    if (HAL_CAN_Transmit(&CAN1_Handler, 10) != HAL_OK)
        return 1; //发送
    return 0;
}

//can口接收数据查询
//buf:数据缓存区;
//返回值:0,无数据被收到;
//		 其他,接收的数据长度;
u8 CAN1_Receive_Msg(u8 *buf)
{
    u32 i;
    if (HAL_CAN_Receive(&CAN1_Handler, CAN_FIFO0, 0) != HAL_OK)
        return 0; //接收数据，超时时间设置为0
    for (i = 0; i < CAN1_Handler.pRxMsg->DLC; i++)
        buf[i] = CAN1_Handler.pRxMsg->Data[i];
    return CAN1_Handler.pRxMsg->DLC;
}

这里面只关注CAN初始化函数u8 CAN1_Mode_Init(u32 tsjw, u32 tbs2, u32 tbs1, u16 brp, u32 mode)，可变参数设置波特率和模式（正常模式和循环模式，我们使用正常模式），波特率计算方法注释中有，在main函数中这样调用： CAN1_Mode_Init(CAN_SJW_1TQ, CAN_BS2_6TQ, CAN_BS1_8TQ, 3, CAN_MODE_LOOPBACK)，从而设置波特率1000K。

CAN接收和发送我们使用官方demo中的函数。

2. 电机数据定义与通讯函数：

创建motor.h文件（以下没有显示无关代码）

#ifndef __MOTOR
#define __MOTOR

#include "stm32f4xx_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		//电机数量
#define CAN_CONTROL		// CAN总线控制
// #define PWM_CONTROL		// PWM控制

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

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

这里面定义了一个电机数据的结构体moto_measure_t，宏定义motor_num是电机数量，可选宏定义CAN_CONTROL和PWM_CONTROL决定是使用CAN控制还是PWM控制。

创建motor.c文件（以下没有显示无关代码）

#include "can.h"
#include "motor.h"

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

/*******************************************************************************************
  * @Func			void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
  * @Brief    接收云台电机,3510电机通过CAN发过来的信息
  * @Param		
  * @Retval		None
  * @Date     2015/11/24
*******************************************************************************************/
u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
	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;
}

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;
	
	if(HAL_CAN_Transmit(&CAN1_Handler,1000)!=HAL_OK) return 1;     //发送
    return 0;	
}	


//CAN中断处理过程
//此函数会被CAN_Receive_IT()调用
//hcan:CAN句柄
void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef *hcan)
{
	if (hcan->Instance == CAN1)
	{
		//CAN_Receive_IT()函数会关闭FIFO0消息挂号中断，因此我们需要重新打开
		__HAL_CAN_ENABLE_IT(&CAN1_Handler, CAN_IT_FMP0); //重新开启FIF00消息挂号中断
		i = hcan->pRxMsg->StdId - 0x201;
		switch(hcan->pRxMsg->StdId){
			case 0x201: 
			case 0x202: 
			case 0x203: 
			case 0x204: 
				if (moto_chassis[i].msg_cnt++ <= 50)
				    get_moto_offset(&moto_chassis[i], hcan);
				else get_moto_measure(&moto_chassis[i], hcan);
				break;
			default:break;
		}
	}
}

这里面定义了一个电机数组moto_chassis[motor_num]与一个电机变量moto_info,定义了两个函数分别进行CAN报文接收与CAN报文发送。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)设置四个电机的电流值。

最后重定义了CAN接收中断回调函数void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef *hcan)，每接收到一帧报文都会进入这个回调函数。通过接收报文的ID，分别将报文信息更新到四个电机数据结构体中。这个地方很重要！！！

3. 电机PID计算函数：

创建motor_pid.h文件：

#ifndef __MOTOR_PID_H
#define __MOTOR_PID_H
#include "stm32f4xx_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);

extern pid_t pid_rol;
extern pid_t pid_pit;
extern pid_t pid_yaw;
extern pid_t pid_pit_omg;
extern pid_t pid_yaw_omg;
extern pid_t pid_spd[4];
extern pid_t pid_yaw_alfa;
extern pid_t pid_chassis_angle;
extern pid_t pid_poke;
extern pid_t pid_poke_omg;
extern pid_t pid_imu_tmp;  //imu_temperature
extern pid_t pid_cali_bby; //big buff yaw
extern pid_t pid_cali_bbp;
extern pid_t pid_omg;
extern pid_t pid_pos;
#endif

创建motor_pid.c文件：

#include "motor_pid.h"
#include "mytype.h"
#include <math.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;
//	
}

/*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);
}

PID的代码就不讲了，请自行阅读。

4. main函数：

好了，现在可以写main函数了。

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "key.h"
#include "lcd.h"
#include "can.h"
#include "motor.h"
#include "motor_pid.h"
#include "timer.h"

/***************************************************************************************************
 描述：M3508 PID控制实验
 功能：通过两个按键控制M3508电机的速度，采用CAN通信进行电机速度闭环控制，使用中断方式接收CAN报文
 测试：正点原子探索者F4板子/阿波罗F7板子（本例程是F4，F7除了库和头文件不一样其他都相同
 注意：使用F1，CAN驱动部分会不一样，对应的需要修改motor.c里面的
			 读取数据函数(get_moto_measure)和
			 写入数据的函数(set_moto_current)
 作者：何为其然 @CSDN (主页：https://blog.csdn.net/qq_30267617)
 时间：2021-08-12
***************************************************************************************************/

int main(void)
{
	u8 key;
	s8 key_cnt;
	u8 i;
	pid_t pid_speed[motor_num];		   //电机速度PID环
	float set_speed_temp;			   //加减速时的临时设定速度
	int16_t delta;					   //设定速度与实际速度的差值
	int16_t max_speed_change = 1000;   //电机单次最大变化速度，加减速用
									   // 500经测试差不多是最大加速区间，即从零打到最大速度不异常的最大值
	static float set_speed[motor_num]; //电机速度全局变量

	HAL_Init();																	 //初始化HAL库
	Stm32_Clock_Init(360, 25, 2, 8);											 //设置时钟,180Mhz
	delay_init(180);															 //初始化延时函数
	uart_init(115200);															 //初始化USART
	LED_Init();																	 //初始化LED
	KEY_Init();																	 //初始化按键
	CAN1_Mode_Init(CAN_SJW_1TQ, CAN_BS2_6TQ, CAN_BS1_8TQ, 3, CAN_MODE_LOOPBACK); //CAN初始化,波特率1000Kbps
	PWM_Init();
	//PID初始化
	for (i = 0; i < 4; i++)
	{
		PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f); //4 motos angular rate closeloop.
	}

	while (1)
	{
		// 按键控制速度加减
		key = KEY_Scan(0);
		if (key == KEY0_PRES) //KEY0按下,发送一次数据
		{
			key_cnt++;
		}
		else if (key == KEY1_PRES) //WK_UP按下，改变CAN的工作模式
		{
			key_cnt--;
		}

#if defined CAN_CONTROL
		if (key_cnt < -16)
			key_cnt = -16;
		if (key_cnt > 16)
			key_cnt = 16;
		set_speed[0] = set_speed[1] = set_speed[2] = set_speed[3] = key_cnt * 500; // -8000-8000，双向转

		//PID采样，获取电机数据
		// 在CAN接收中断中更新数据

		//PID计算输出，写5 多算了一个，避免最后一个电机不计算，这里是一个未知bug
		for (i = 0; i < 5; i++)
		{
			//PID计算

			// 无加减速
			//pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, set_speed[i]);

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

		// 通过串口调试助手观察实际速度与设定速度
		printf("PID out: %f\r\n", pid_speed[0].pos_out);
		printf("real speed: %d \r\n", moto_chassis[0].speed_rpm);
		printf("set speed: %f \r\n", set_speed[0]);

		//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));

#elif defined PWM_CONTROL
		if (key_cnt < 0)
			key_cnt = 0;
		if (key_cnt > 20)
			key_cnt = 20;
		set_speed[0] = set_speed[1] = set_speed[2] = set_speed[3] = key_cnt * 100; // 0-2000,单向转
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, set_speed[0]);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, set_speed[1]);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, set_speed[2]);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, set_speed[3]);

#endif

		delay_ms(5); // 采样周期
	}
}

主函数的代码流程如下，具体代码就不讲了。

在这里插入图片描述

五. 官方代码移植-查询接收

除了中断接收的方式，还可以使用查询方式进行CAN报文接收，此时的main函数代码如下：

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "key.h"
#include "lcd.h"
#include "can.h"
#include "motor.h"
#include "motor_pid.h"
#include "timer.h"

int main(void)
{
	u8 key;
	s8 key_cnt;
	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 = 1000;   //电机单次最大变化速度，加减速用
									   // 500经测试差不多是最大加速区间，即从零打到最大速度不异常的最大值
	static float set_speed[motor_num]; //电机速度全局变量

	HAL_Init();																	 //初始化HAL库
	Stm32_Clock_Init(360, 25, 2, 8);											 //设置时钟,180Mhz
	delay_init(180);															 //初始化延时函数
	uart_init(115200);															 //初始化USART
	LED_Init();																	 //初始化LED
	KEY_Init();																	 //初始化按键
	CAN1_Mode_Init(CAN_SJW_1TQ, CAN_BS2_6TQ, CAN_BS1_8TQ, 3, CAN_MODE_LOOPBACK); //CAN初始化,波特率1000Kbps
	PWM_Init();
	//PID初始化
	for (i = 0; i < 4; i++)
	{
		PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f); //4 motos angular rate closeloop.
	}

	while (1)
	{
		// 按键控制速度加减
		key = KEY_Scan(0);
		if (key == KEY0_PRES) //KEY0按下,发送一次数据
		{
			key_cnt++;
		}
		else if (key == KEY1_PRES) //WK_UP按下，改变CAN的工作模式
		{
			key_cnt--;
		}

#if defined CAN_CONTROL
		if (key_cnt < -16)
			key_cnt = -16;
		if (key_cnt > 16)
			key_cnt = 16;
		set_speed[0] = set_speed[1] = set_speed[2] = set_speed[3] = key_cnt * 500; // -8000-8000，双向转

		//PID采样，获取电机数据
		retry = 0;
		flag_motor[0] = flag_motor[1] = flag_motor[2] = flag_motor[3] = 0;
		while (retry < 20)
		{
			HAL_CAN_Receive(&CAN1_Handler,CAN_FIFO0,1000);//接收数据	
			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[1] && flag_motor[2] && flag_motor[3])
				break;
			else
				retry++;
		}

		//PID计算输出，写5 多算了一个，避免最后一个电机不计算，这里是一个未知bug
		for (i = 0; i < 5; i++)
		{
			//PID计算

			// 无加减速
			//pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, set_speed[i]);

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

		// 通过串口调试助手观察实际速度与设定速度
		printf("PID out: %f\r\n", pid_speed[0].pos_out);
		printf("real speed: %d \r\n", moto_chassis[0].speed_rpm);
		printf("set speed: %f \r\n", set_speed[0]);

		//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));

#elif defined PWM_CONTROL
		if (key_cnt < -10)
			key_cnt = -10;
		if (key_cnt > 10)
			key_cnt = 10;
		set_speed[0] = set_speed[1] = set_speed[2] = set_speed[3] = key_cnt * 50; // -500-500,双向转
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_1, 1500+set_speed[0]);				// PWM范围1000-2000,双向转
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_2, 1500+set_speed[1]);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_3, 1500+set_speed[2]);
		__HAL_TIM_SetCompare(&htim5, TIM_CHANNEL_4, 1500+set_speed[3]);

#endif

		delay_ms(5); // 采样周期
	}
}

与前面中断接收的唯一不同就是将读取CAN报文放到了while(1)循环中，在每个控制周期内主动读电机信息（调用HAL_CAN_Receive(&CAN1_Handler,CAN_FIFO0,1000)函数）。另外注意同时关闭CAN接收中断（在can.h中定义#define CAN1_RX0_INT_ENABLE 0）

何为其然

关注

167
点赞
踩
1082

收藏

觉得还不错? 一键收藏
打赏
51
评论
大疆M3508电机使用CAN通信进行速度PID闭环控制详解

目录一. 简介二. 电机通信协议三. 电机PID控制原理四. 官方代码移植-中断接收五. 官方代码移植-查询接收一. 简介之前写过一篇文章STM32实现四驱小车（五）电机控制任务——电机速度PID控制算法，其中是以大疆的M3508电机为例进行讲解的（没错，就是RoboMaster机器人同款电机，不过Robomaster上的电机好像是小一号的M2006）。不少小伙伴私信问我要代码，我都回复说不是有官方demo么。后来问的人多了我大概明白了，看来官方的Demo还是有点门槛。可能是带FreeRTOS操作系统看
复制链接

扫一扫