一. 简介
之前写过一篇文章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查询接收与中断接收两种方式，无操作系统
下载链接：
二. 电机通信协议
关于电机的说明请参照电机配套的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)
{
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;
}
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消息挂号中断
switch(hcan->pRxMsg->StdId){
case 0x201: get_moto_measure(&moto_chassis[0], hcan); break;
case 0x202: get_moto_measure(&moto_chassis[1], hcan); break;
case 0x203: get_moto_measure(&moto_chassis[2], hcan); break;
case 0x204: get_moto_measure(&moto_chassis[3], 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.
}
// 等待CAN通讯成功
while (!get_moto_measure(&moto_info, &CAN1_Handler))
{
LED0 = ~LED0;
delay_ms(200);
}
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.
}
// 等待CAN通讯成功
while (!get_moto_measure(&moto_info, &CAN1_Handler))
{
LED0 = ~LED0;
delay_ms(200);
}
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)
{
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)循环中，在每个控制周期内主动读电机信息。另外注意同时关闭CAN接收中断（在can.h中定义#define CAN1_RX0_INT_ENABLE 0）