代码收藏家技术教程 2025-02-16

STM32 3508电机的pid速度闭环

最近又接触了robomaster比赛要用到的大疆3508（stm32f407）

只是简单的用CAN发收数据的速度闭环

先贴上基本资料（后有博主链接）

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

CAN上传报文：

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

标识符：0×200+电调ID (如：ID为1,该标识符为0×201)（ID值位C620绿灯亮的次数）
帧类型：标准帧
帧格式：DATA
DLC:8字节

（角度环读取DATA[0],DATA[1] 速度环读取DATA[2]，DATA[3]）

这还是很重要的

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

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

我们的目标是控制速度，上图变成下面这样：

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

由于速度换输出的是电流值，所以这也解释了为什么单片机通过CAN报文下发的是转矩电流。至于电流环在哪？那是硬件决定的。
原文链接：https://blog.csdn.net/qq_30267617/article/details/119651826

这篇文章只写了速度闭环

贴上代码具体代码解释

CAN.C

#include "can.h"
#include "Encorde.h"

//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+1+tbs2+1+1)*brp);
//mode:CAN_Mode_Normal,普通模式;CAN_Mode_LoopBack,回环模式;
//Fpclk1的时钟在初始化的时候设置为42M,如果设置CAN1_Mode_Init(CAN_SJW_1tq,CAN_BS2_6tq,CAN_BS1_7tq,6,CAN_Mode_LoopBack);
//则波特率为:42M/((6+7+1)*6)=500Kbps
//返回值:0,初始化OK;
// 其他,初始化失败;
void CAN_Configure()
{
CAN_InitTypeDef can; //CAN结构体
CAN_FilterInitTypeDef can_filter; //筛选器结构体
GPIO_InitTypeDef GPIO;
NVIC_InitTypeDef nvic;

RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//系统总线高性能模块(如CPU、DMA和DSP等)之间的连接
RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);//低带宽的周边外设之间的连接,例如UART

GPIO_PinAFConfig(GPIOA, GPIO_PinSource11, GPIO_AF_CAN1);
GPIO_PinAFConfig(GPIOA, GPIO_PinSource12, GPIO_AF_CAN1);

GPIO.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12;
GPIO.GPIO_Mode = GPIO_Mode_AF;
GPIO_Init(GPIOA, & GPIO);

nvic.NVIC_IRQChannel = CAN1_RX0_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = 2;
nvic.NVIC_IRQChannelSubPriority = 1;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);

nvic.NVIC_IRQChannel = CAN1_TX_IRQn;
nvic.NVIC_IRQChannelPreemptionPriority = 1;
nvic.NVIC_IRQChannelSubPriority = 1;
nvic.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&nvic);

CAN_DeInit(CAN1);
CAN_StructInit(&can);

can.CAN_TTCM = DISABLE; //非时间触发通信模式
can.CAN_ABOM = DISABLE; //软件自动离线管理
can.CAN_AWUM = DISABLE; //睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
can.CAN_NART = DISABLE;//禁止报文自动传送
can.CAN_RFLM = DISABLE;//报文不锁定,新的覆盖旧的
can.CAN_TXFP = ENABLE;   //优先级由报文标识符决定
can.CAN_Mode = CAN_Mode_Normal;//四个模式正常模式
can.CAN_SJW = CAN_SJW_1tq;//重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1tq~CAN_SJW_4tq
can.CAN_BS1 = CAN_BS1_9tq;//Tbs1范围CAN_BS1_1tq ~CAN_BS1_16tq
can.CAN_BS2 = CAN_BS2_4tq;//Tbs2范围CAN_BS2_1tq ~   CAN_BS2_8tq
can.CAN_Prescaler = 3; //CAN BaudRate 42/(1+9+4)/3=1Mbps
CAN_Init(CAN1, &can); //初始化CAN1

can_filter.CAN_FilterNumber=0; //过滤器0
can_filter.CAN_FilterMode=CAN_FilterMode_IdMask;
can_filter.CAN_FilterScale=CAN_FilterScale_32bit;//32
can_filter.CAN_FilterIdHigh=0x0000; //32位ID高八位低八位
can_filter.CAN_FilterIdLow=0x0000;
can_filter.CAN_FilterMaskIdHigh=0x0000;//32位高低八位MASK
can_filter.CAN_FilterMaskIdLow=0x0000;
can_filter.CAN_FilterFIFOAssignment=0;//the message which pass the filter save in fifo0
can_filter.CAN_FilterActivation=ENABLE;//激活过滤筛选器0
CAN_FilterInit(&can_filter); //滤波器初始化

CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE);
CAN_ITConfig(CAN1,CAN_IT_TME,ENABLE);
}
/**
*****************************************************************************
* 函数名： can发送中断电调发送给单片机
******************************************************************************
*/
void CAN1_TX_IRQHandler(void) //CAN TX
{
if (CAN_GetITStatus(CAN1,CAN_IT_TME)!= RESET)
   {
       CAN_ClearITPendingBit(CAN1,CAN_IT_TME);
}
}
/**
*****************************************************************************
*函数名： can接收中断单片机发送电调
******************************************************************************
*/
CanRxMsg rx_message;
void CAN1_RX0_IRQHandler(void)
{

if (CAN_GetITStatus(CAN1,CAN_IT_FMP0)!= RESET)
   {
CAN_ClearITPendingBit(CAN1, CAN_IT_FF0);
       CAN_ClearFlag(CAN1, CAN_FLAG_FF0);
       CAN_Receive(CAN1, CAN_FIFO0, &rx_message);
       CanReceiveMsgProcess(&rx_message);
}
}

void Set_ChassisMotor_Current(int16_t cm1_iq, int16_t cm2_iq, int16_t cm3_iq, int16_t cm4_iq)
{
CanTxMsg tx_message;
tx_message.StdId = 0x200;
tx_message.IDE = CAN_Id_Standard;
tx_message.RTR = CAN_RTR_Data;
tx_message.DLC = 0x08;

tx_message.Data[0] = (uint8_t)(cm1_iq >> 8); //cm1_iq 234 分别视自己ID设值
tx_message.Data[1] = (uint8_t)cm1_iq;
tx_message.Data[2] = (uint8_t)(cm2_iq >> 8);
tx_message.Data[3] = (uint8_t)cm2_iq;
tx_message.Data[4] = (uint8_t)(cm3_iq >> 8);
tx_message.Data[5] = (uint8_t)cm3_iq;
tx_message.Data[6] = (uint8_t)(cm4_iq >> 8); //ID为4 则调用时只需要设置cm4_iq 为 CM_LF_Speedloop.Out（PID返回值）
tx_message.Data[7] = (uint8_t)cm4_iq;
CAN_Transmit(CAN1,&tx_message);
}

CAN.h

/**
******************************************************************************
* 文件名： can底层驱动文件头文件
* 函数列表：can配置函数
*   can接收函数
*           can接收中断
******************************************************************************
*/

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

//地盘四个电机的can id
#define CAN_ID_CM1 0x201
#define CAN_ID_CM2 0x202
#define CAN_ID_CM3 0x203
#define CAN_ID_CM4 0x204

void CAN_Configure(void);
void CanReceiveMsgProcess(CanRxMsg *message);
void CAN1_RX0_IRQHandler(void);
void CAN1_TX_IRQHandler(void) ;//CAN TX;

#endif

Encorde.c

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

volatile Encoder CM1Encoder = {0,0,0,0,0,0,0,0,0};
volatile Encoder CM2Encoder = {0,0,0,0,0,0,0,0,0};
volatile Encoder CM3Encoder = {0,0,0,0,0,0,0,0,0};
volatile Encoder CM4Encoder = {0,0,0,0,0,0,0,0,0};
volatile Encoder GMYawEncoder = {0,0,0,0,0,0,0,0,0};
volatile Encoder GMPitchEncoder = {0,0,0,0,0,0,0,0,0};

void CanReceiveMsgProcess(CanRxMsg *message)
{
   switch(message->StdId)
   {
       case CAN_ID_CM4: //ID不同时记得gai'bian
       {
               getEncoderData(&CM4Encoder, message);
       }break;
       case CAN_ID_PITCH:
       {
               getEncoderData(&GMPitchEncoder, message);
       }break;
   }
}

void getEncoderData(volatile Encoder *v, CanRxMsg * msg) // 接收3508电机通过CAN发过来的信息
{
   int i=0;
   int32_t temp_sum = 0;
   v->last_raw_value = v->raw_value;

   v->raw_value = (msg->Data[2]<<8)|msg->Data[3];
   v->diff = v->raw_value – v->last_raw_value;

   if(v->diff < -7500) //两次编码器的反馈值差别太大，表示圈数发生了改变
   {
       v->round_cnt++;
       v->ecd_raw_rate = v->diff + 8192;
   }
   else if(v->diff>7500)
   {
       v->round_cnt–;
       v->ecd_raw_rate = v->diff – 8192;
   }
   else
   {
       v->ecd_raw_rate = v->diff;
   }

   //计算得到连续的编码器输出值
   v->ecd_value = v->raw_value + v->round_cnt * 8192;
   //计算得到角度值，范围正负无穷大
   v->ecd_angle = (float)(v->raw_value – v->ecd_bias)*360/8192 + v->round_cnt * 360;
   v->rate_buf[v->buf_count++] = v->ecd_raw_rate;
   if(v->buf_count == RATE_BUF_SIZE)
   {
       v->buf_count = 0;
   }
   //计算速度平均值
   for(i = 0;i < RATE_BUF_SIZE; i++)
   {
       temp_sum += v->rate_buf[i];
   }
   v->filter_rate = (int32_t)(temp_sum/RATE_BUF_SIZE);
}

void TIM3_Int_Init(void) //配置定时器中断
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
NVIC_InitTypeDef NVIC_InitStructure;

RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE); //使能相应的时钟

TIM_TimeBaseInitStructure.TIM_Period =99;
TIM_TimeBaseInitStructure.TIM_Prescaler=839;
TIM_TimeBaseInitStructure.TIM_CounterMode=TIM_CounterMode_Up;
TIM_TimeBaseInitStructure.TIM_ClockDivision=TIM_CKD_DIV1; //分频 //10ms(0.01s)中断一次

TIM_TimeBaseInit(TIM3,&TIM_TimeBaseInitStructure);

TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);

NVIC_InitStructure.NVIC_IRQChannel=TIM3_IRQn; //中断
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;
NVIC_InitStructure.NVIC_IRQChannelSubPriority=3;
NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE; //使能中断
NVIC_Init(&NVIC_InitStructure);

TIM_SetCounter(TIM3,0); //定时器3清零
TIM_Cmd(TIM3, ENABLE);
}

void TIM3_IRQHandler(void) //通过溢出中断来读取圈数
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
{
ChassisMotor_Ctrl();
ChassisMotor_Out();
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}

Encorde.h

#ifndef _ENCORDE_
#define _ENCORDE_

#include "stm32f4xx.h"

#define RATE_BUF_SIZE 6
typedef struct
   {
   int32_t raw_value;                    //编码器不经处理的原始值
   int32_t last_raw_value;                   //上一次的编码器原始值
   int32_t ecd_value; //经过处理后连续的编码器值
   int32_t diff;                           //两次编码器之间的差值
   int32_t temp_count; //计数用
   uint8_t buf_count;                       //滤波更新buf用
   int32_t ecd_bias;                       //初始编码器值
   int32_t ecd_raw_rate;                   //通过编码器计算得到的速度原始值
   int32_t rate_buf[RATE_BUF_SIZE]; //buf，for filter
   int32_t round_cnt;                       //圈数
   int32_t filter_rate;                   //速度
   float ecd_angle;                       //角度
}Encoder;

void CanReceiveMsgProcess(CanRxMsg *message); //接收3508电机通过CAN发过来的信息
void getEncoderData(volatile Encoder *v, CanRxMsg * msg); //得到编码器数据

void Quad_Encoder_Configuration(void); //初始化定时器4
int32_t GetQuadEncoderDiff(void); //得到圈数
void Encoder_Start(void);

motor.c

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

ChassisMotorPID CM_LF_Speedloop;
extern volatile Encoder CM4Encoder;
int b;
/**
*****************************************************************************
*函数名： 3508电机配置函数
*函数功能描述：用于配置化电机有关的各项参数
*函数参数：无
*函数返回值：无
******************************************************************************
*/
void ChassisMotor_Configure()
{
CM_LF_Speedloop.Kp = 1.0f;
   CM_LF_Speedloop.Ki = 3.0f;
   CM_LF_Speedloop.Kd = 0.0f;
   CM_LF_Speedloop.PoutMax = 5000.0f;
   CM_LF_Speedloop.IoutMax = 100.0f;
   CM_LF_Speedloop.DoutMax = 1000.0f;
   CM_LF_Speedloop.OutMax = 7000.0f;
   CM_LF_Speedloop.Set = 800;
   CM_LF_Speedloop.Real = 0.0f;
   CM_LF_Speedloop.Out = 0.0f;
   CM_LF_Speedloop.err = 0.0f;
   CM_LF_Speedloop.err_last = 0.0f;
   CM_LF_Speedloop.err_llast = 0.0f;
   CM_LF_Speedloop.integral = 0.0f;
}
/**
*****************************************************************************
*函数名： 3508电机控制函数
*函数功能描述：用于pid的实际计算定时调用，周期4ms 实际值依靠编码器
*函数参数：无
******************************************************************************
*/
void ChassisMotor_Ctrl()
{
float Pout = 0.0f;
   float Iout = 0.0f;
   float Dout = 0.0f;

// 电机速度环
CM_LF_Speedloop.Real = CM4Encoder.filter_rate;

   CM_LF_Speedloop.err_last = CM_LF_Speedloop.err;
   CM_LF_Speedloop.err = CM_LF_Speedloop.Set – CM_LF_Speedloop.Real;
   CM_LF_Speedloop.integral += CM_LF_Speedloop.err;

   Pout = CM_LF_Speedloop.Kp * CM_LF_Speedloop.err;
   Pout = Pout < CM_LF_Speedloop.PoutMax ? Pout : CM_LF_Speedloop.PoutMax;
   Pout = Pout > -CM_LF_Speedloop.PoutMax ? Pout : -CM_LF_Speedloop.PoutMax;

   Iout = CM_LF_Speedloop.Ki * CM_LF_Speedloop.integral;
   Iout = Iout < CM_LF_Speedloop.IoutMax ? Iout : CM_LF_Speedloop.IoutMax;
   Iout = Iout > -CM_LF_Speedloop.IoutMax ? Iout : -CM_LF_Speedloop.IoutMax;

   Dout = CM_LF_Speedloop.Kd * (CM_LF_Speedloop.err – CM_LF_Speedloop.err_last);
   Dout = Dout < CM_LF_Speedloop.DoutMax ? Dout : CM_LF_Speedloop.DoutMax;
   Dout = Dout > -CM_LF_Speedloop.DoutMax ? Dout : -CM_LF_Speedloop.DoutMax;

   CM_LF_Speedloop.Out = Pout + Iout + Dout;
   CM_LF_Speedloop.Out = CM_LF_Speedloop.Out < CM_LF_Speedloop.OutMax ? CM_LF_Speedloop.Out : CM_LF_Speedloop.OutMax;
   CM_LF_Speedloop.Out = CM_LF_Speedloop.Out > -CM_LF_Speedloop.OutMax ? CM_LF_Speedloop.Out : -CM_LF_Speedloop.OutMax;

}
/**
*****************************************************************************
*函数名： 3508电机输出函数
*函数功能描述：执行pid解算出的数据
*函数参数：无
******************************************************************************
*/
void ChassisMotor_Out()
{
   b=CM_LF_Speedloop.Out;
Set_ChassisMotor_Current(0,0, 0,CM_LF_Speedloop.Out);
}

motor.h

/*********************************************************************************
*Copyright(C),SLDX->Ambition
*文件名: 地盘电机头文件
           本文件的函数依赖于CAN头文件和Encoder头文件，主要利用其获取和发送数据
           到电机。本文件主要通过定时调用ChassisMotor_Ctrl函数进行pid解算，通过
           调用ChassisMotor_Out函数进行实际输出。ChassisMotor_Configure进行初始
           化电机各项参数
*主要函数列表:
1.ChassisMotor_Configur
2.ChassisMotor_Ctrl
   3.ChassisMotor_Out
**********************************************************************************/

#ifndef _MOTOR_
#define _MOTOR_

typedef struct ChassisMotorPID
{
   float Kp,Ki,Kd; //定义
   float PoutMax,IoutMax,DoutMax;
   float OutMax;

   float Set; //定义设定值
   float Real; //编码器采样值
float Out; //输出值

float err; //定义偏差值
float err_last; //上一次偏差值
float err_llast; //最上次偏差值
   float integral; //误差累计
}ChassisMotorPID;

extern ChassisMotorPID CM_LF_Speedloop;

void ChassisMotor_Configure(void); //pid解算
void ChassisMotor_Ctrl(void); //初始化电机各项参数
void ChassisMotor_Out(void); //实际输出

#endif

main.c

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "can.h"
#include "motor.h"
#include "encorde.h"

int main(void)
{
   u8 canbuf[8];
   delay_init(168);                                                      //延时函数初始化
   NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);   //设置中断优先级分组为组2：2位抢占优先级，2位响应优先级
Quad_Encoder_Configuration();
TIM3_Int_Init();
1 CAN_Configure(); //can初始化

   ChassisMotor_Configure();
//   Set_ChassisMotor_Current(0, 0, 0, 500); //不能一开始直接给定值！！会让电机一直以最大速度转！！注释掉这行代码
    while(1)
   {
   }
}

所需的核心代码粘贴完毕啦

我再理一理思路！

1. 在20ms一次中断的定时器里进行每次计算出pid的计算值 CM_LF_Speedloop.Out

再在 Set_ChassisMotor_Current(0,0, 0,CM_LF_Speedloop.Out); 中将 CM_LF_Speedloop.Out

通过Set_ChassisMotor_Current（）函数以can形式在CAN1_RX0_IRQHandler //can接受中断中发送给电调电调收到后在can发送中断CAN1_TX_IRQHandler中发送给电机后电机通过接受值来调节电流

2. 电机是如何收到电机真实值

CM_LF_Speedloop.Real = CM4Encoder.filvolatile

void CanReceiveMsgProcess(CanRxMsg *message)

getEncoderData(&CM4Encoder, message);

Encoder CM4Encoder = {0,0,0,0,0,0,0,0,0};ter_rate;

v->raw_value = (msg->Data[2]<<8)|msg->Data[3];

作者：JUDITH-99