GM6020单闭环控制的实现

前言

最近也是开始调上gm6020了，然后调试过程中也算是遇到了很多挫折，但最后还是解决了。便以此文章复盘一下本次的工程。简单介绍一下本次调试电机：gm6020电机是一款内部集成驱动器的高可靠性直流无刷电机，可广泛应用于机器人比赛、科研教育、 自动化设备等领域。其详细资料也是在大疆官网（RoboMaster 超级对抗赛）上能查到，这里也不过多赘述。然后直接进入我们的主题。

问题引出

发现问题：

我们的目标是使电机转起来然后达到一个稳定值，但是如何使电机转？如何使电机转的稳定？难道是和小学科学课中南孚电池直接接电机正负极转起来吗？当然不是，这时候我们得先有一个大致的思路：就是电机得有个能接收数据和发送数据的控制器，该控制器通过接收用户发出的数据并经过处理发送给电机，如何实现这一过程呢？接收到了信号以后，电机该如何调节自身使其达到用户预设的那个值呢？

明确问题

我们可以把问题主要拆解成三个部分：1.编写代码实现控制器（C板）接收用户数据，并发送给电机。2.用一种算法使电机不断的向目标值靠拢，为了更好的调试，我们配置freerrtos。3.用遥控器实现对电机的控制

然后我们再将要实现的功能逐一细化：1.对于第一个部分我们的电机首先得配置CAN通信，在代码中对电机接收数据进行初始化操作，大体如下：1）配置过滤器2）电机接收报文和电机反馈报文3）编写中断回调函数使其能读取到电机数据4）编写向电机写入数据的函数

2.对于2功能的要求，高中学过通用技术这一门课的同学一下子就想到了闭环控制（死去的知识开始攻击...）那么如何实现闭环控制呢？那就用到了PID算法，稍后我们也会对该算法进行简单的讲解。

3.遥控器与电机的联系又用到了DMA的控制，对于这部分我们也要编写相应的接收与发送代码。

 

明确了这三个部分的要求，我们便开始详细的从理论到实践这两个方面开始介绍：

第一部分：

配置can口

什么是can通信？下面链接我认为比较好懂的，可以大致了解一下通俗易懂“can通信”也就是说这是一种协议能使单片机控制电机，那下面我们就从cubemx配置。

先完成默认操作在Systemcore--SYS--Debug中改成Serial Wire 时钟源随便配一个TIM1

IDE改为MDK-ARM  、时钟—HLCK改为168hz、以及code generate勾选产生c\h文件

（为了使思路清晰，以下cubemx的配置是分开配置的，实际上应该一次性配置完）

再打开Conectivity--CAN1 中断设置打开Rx0、其中还要额外找到PD1和PD0分别设置CAN1_tx和CAN1_rx

然后打开USART1和USART3二者都为异步收发模式，后者波特率改为100000bit/s word length改为9bits parity改为Even stop bits改为1

由于我们是要写一个中断回调函数，用于接收单片机收到的数据，所以我们得在CAN1将RX0 interrupts勾选上

然后我们的can口就配置的差不多了，接下来进行电机接收发数据于发送数据的代码编写

电机接收于发送原理：

之前我们简单提过了CAN通信，也明白之间的联系是通过报文进行收发，文字这里也不多赘述，具体可观察下表：

代码部分：

定义数据接收结构体

我们首先是做电机数据接收，所以得在头文件中定义个结构体Motor6020rx用于接收电机读到的数据，其中包括变量speed、angle、temperature、current

typedef struct 
{
    int16_t speed;   
    uint16_t angle;
    uint8_t temperature;
    int16_t current;
}Motor6020rx;

 配置过滤器

然后我们再对电机接收方面进行代码编写。先进行过滤器配置CAN的过滤器的目的是很容易理解的，由于总线上的信号是以广播的形式发送的，如果设备都在对于每一个被广播的信号都进行接收+判断，那么势必会浪费大量的时间在这项其实没有什么意义的工作上，解决的方法就是通过设置过滤器，屏蔽掉一些和自己无关的设备发来的信息。

•CAN的过滤器模式分为掩码模式和列表模式，列表模式简单来说就是制作一张ID表，如果来的数据的ID在这张表中则接收，否则不收。

•重点介绍一下掩码模式的原理：掩码模式的思路很容易理解，举个例子，某所学校的学号构成方式为[4位10进制 入学年份]+[4位10进制 学生序号]，比如一个2016年入学的学生，其学号可以是20161234，那么假如要开一个2016年毕业生的庆祝会，会场门口要检查每一个人的学号，只有2016级的才可以进入，这里应该使用什么样的判断方法呢？

•首先，我们需要设置屏蔽码，屏蔽掉后四位的学生序号，因为他们和本次检测无关，反而增大了计算量。

•然后设置检验码2016，如果屏蔽后的结果等于2016，则可以放行。

•如下表所示，第一行为原码，第二行为掩码，将第一行表格中的数与掩码相乘，即得到第三行的屏蔽码，最后一行是验证码，屏蔽码和验证码比较确定一致后，就接收该学号。

•如下表所示，第一行为原码，第二行为掩码，将第一行表格中的数与掩码相乘，即得到第三行的屏蔽码，最后一行是验证码，屏蔽码和验证码比较确定一致后，就接收该学号。

（上述部分取自同济大学SuperPower战队培训资料）

•那么我们来看一下代码中验证码和屏蔽码这两项的配置，屏蔽码设为0x00000000，无论任何标识符通过之后都变成0x00000000，验证码为0x00000000，所以无论任何屏蔽码都能通过。可见其实并没有起到任何过滤作用，这是因为CAN总线上挂载的几个电调，我们的主控都需要接收其数据，所以无论来的标识符是哪个，都要照单全收，而CAN不配置完过滤器是无法开启的，所以才有这套验证码+屏蔽码都是0x00000000的操作。

void can_filter_init(void) {
    CAN_FilterTypeDef cf;
    cf.FilterBank = 0; 
    cf.SlaveStartFilterBank = 14;
    cf.FilterMode = CAN_FILTERMODE_IDMASK;
    cf.FilterScale = CAN_FILTERSCALE_32BIT;
    cf.FilterIdHigh = 0x0000;
    cf.FilterIdLow = 0x0000;
    cf.FilterMaskIdHigh = 0x0000;
    cf.FilterMaskIdLow = 0x0000;
    cf.FilterFIFOAssignment = CAN_FILTER_FIFO0;
    cf.FilterActivation = ENABLE;
    HAL_CAN_ConfigFilter(&hcan1, &cf);
	  HAL_CAN_ActivateNotification(&hcan1,CAN_IT_RX_FIFO0_MSG_PENDING);
    HAL_CAN_Start(&hcan1);
		
}

编写电机接收函数和中断回调函数

void motor_read(Motor6020rx *motor,uint8_t *data){

    motor->angle=(data[0]<<8)|data[1];
    motor->speed=(data[2]<<8)|data[3];
    motor->current=(data[4]<<8)|data[5];
    motor->temperature=data[6];
}

这一段函数的功能是单片机和电机内部的电调构建联系，我们知道6020是高度集成的电机，即电调是集成在电机内部，所以我们得查阅电机的官方手册对其ID进行接收以及发送

void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan){
    CAN_RxHeaderTypeDef RxHeader;
    uint8_t RxData[8];
    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &RxHeader, RxData);

    if(RxHeader.StdId == 0x205){
        motor_read(&motor6020rx,RxData);

    }   
}

简单提一下stm32中“中断”的概念，“中断”：顾名思义是指停下一件事情去做另一件事的过程。（参考中断概念理解）那么在这段中断回调函数呢主要在读取到电机的ID后执行motor_read，在本次实验对照上面表中我的电机id是1也就是0x205。

编写电机发送数据函数

HAL_StatusTypeDef motor_write(uint32_t stdid,int16_t motor1,int16_t motor2,int16_t motor3,int16_t motor4)

其中我们需要关注的是括号内的四个参数，其中stdid是电机标识符，后面motor1~4也相当于电机的编号所填写参数对应得位置

HAL_StatusTypeDef motor_write(uint32_t stdid,int16_t motor1,int16_t motor2,int16_t motor3,int16_t motor4){
    TxHeader.StdId = stdid;
    TxHeader.IDE = CAN_ID_STD;
    TxHeader.RTR = CAN_RTR_DATA;
    TxHeader.DLC = 0x08;
    
    can_send[0] = motor1>>8;
    can_send[1] = motor1;
    can_send[2] = motor2>>8;
    can_send[3] = motor2;
    can_send[4] = motor3>>8;
    can_send[5] = motor3;
    can_send[6] = motor4>>8;
    can_send[7] = motor4;

    return HAL_CAN_AddTxMessage(&hcan1, &TxHeader, can_send, (uint32_t*)CAN_TX_MAILBOX0);
}

这段代码大部分也是从官方移植的，只需要知道实现了什么功能即可。接下来我们来介绍下代码内核也就是PID控制算法

第二部分：

PID理论部分：

pid部分是我本人面对这次任务感到最为困难的时候，因为大一还没有上到自动控制原理，完全是面对一个全新的知识点。看过很多优秀学长制作的教学视频例如：华南虎队的视频。都值得去做一个了解

然后这些举例大部分的特点都是从一个例子入手：这里我也举一个我认为讲的比较好的例子。

假如平地上有一个小球（只讨论x轴一个方向）你要用手来回去推动小球到一指定的地点（就是一个点）但你又无法精准的控制手的力道使小球到达这个点，所以你会不断的去推，得到的反馈就是过了这个点或者没有过这个点，你的大脑不断接收小球的位置，通过小球的实时位置和预期位置计算出一个误差Error，从而不断控制手的力道使小球减小误差从而达到一个更接近地点的值。下面是我认为一个比较好的pid教学网址PID调参网站，里面能简单的带你入门一下。

从上面的一个介绍学过通用技术的你就意识到：这是一个闭环控制。

也许我们控制小球过程没那么复杂，在现实中，扰动量无非是地面的摩擦力以及空气阻力，那么，我们如何去减小这种误差呢？

那我们就得搬出PID算法，由于本人也是浅尝辄止，所以只能做到一个简单的介绍。

PID就是“比例（proportional）、积分（integral）、微分（derivative）”然后我们的误差就是Error=预期值-实时值（以下简写为e）。P：是比例顾名思义对误差e进行一个成比例的控制。kp越小，系统反应越慢，但能比较好的达到一个目标值。kp过大，系统反应越快，但达到目标值的效果不是特别好，并且会降低系统的稳定性。

I：积分环节是对误差进行积分，ki越大，消除误差的时间就越短，越快的达到目标值，但是过大的ki也会对系统产生较大的误差与震荡，导致系统的反应性降低。

D:微分我们高中学到就相当于求导，反应的是偏差量变化的一种趋势，可以根据偏差的趋势从而做出减小超调，克服振荡的作用。

用数学公式来展现这一过程：

回到我刚开始做的这个问题：PID将如何作用于系统？便是我们根据电机的响应来调Kp、Ki、Kd这三个参数，来观察电机的波形是否达到我的需求。

理论简单介绍一下就好，毕竟我也没系统学过。

PID代码部分：

首先我们得定义两个结构体pid_def和motor_t其中pid_def中存放了pid的基础数据（下面代码会展现）而motor_t则是电机的speed和angle以及设定值两个量。因为这次做的是单闭环，也就是一套pid来控制角度或者速度这个量，所以我们真正调试的时候仅需要把其中一个变量名改为另一个即可。下面是pid.h

#ifndef __PID_H__
#define __PID_H__

#define PID_KP 0.0f
#define PID_KI 0.0f
#define PID_KD 0.0f
#define PID_MAX_OUTPUT 20000.0f
#define PID_MAX_IOUT 20000.0f

typedef struct  
{
	
	float kp;
	float ki;
	float kd;
	
	float maxout;//ji fen xiane
	float maxiout;//shu chu xiane
	float p_out;
	float i_out;
	float d_out;
	float output;
	
	float set;
	float fdb;
	float err[3];
	
}pid_def;

typedef struct {
    pid_def motor_pid[2];//speed,angle
    float angle;
    float speed;
    float angle_set;
    float speed_set;
    float give_current;
}motor_t;

然后再在pid.c的对pid算法进行初始化和编写pid的计算函数。

#include "pid_h.h"
#include <stdio.h>


void pid_init(pid_def *pid, float kp, float ki, float kd, float maxout, float maxiout){//pid hanshu chushihua
    if(pid==NULL)
        return;
    pid->kp=kp;
    pid->ki=ki;
    pid->kd=kd;
    pid->maxout=maxout;
    pid->maxiout=maxiout;
    pid->err[0]=0;
    pid->err[1]=0;
    pid->err[2]=0;
    pid->p_out=0;
    pid->i_out=0;
    pid->d_out=0;
}


float pid_calc(pid_def *pid, float set, float fdb,int max_angle){
    if(pid== NULL)
        return 0;
    pid->err[2]=pid->err[1];
    pid->err[1]=pid->err[0];
    pid->err[0]=set - fdb;

    pid->set=set;
    pid->fdb=fdb;
    
    
    pid->p_out=pid->kp*pid->err[0];
    pid->i_out+=pid->ki*pid->err[0];
    if(pid->i_out>pid->maxiout)
        pid->i_out=pid->maxiout;
    else if(pid->i_out<-pid->maxiout)
        pid->i_out=-pid->maxiout;
    
    pid->d_out=pid->kd*(pid->err[0]-pid->err[1]);
    

    pid->output=pid->p_out+pid->i_out+pid->d_out;
    if(pid->output>pid->maxout)
        pid->output=pid->maxout;
    else if(pid->output<-pid->maxout)
        pid->output=-pid->maxout;
    
    return pid->output;
}

以上代码也并非我本人手搓，但是做到理解功能以及每一步作用即可。接下来配置freertos

配置FREERTOS

 FreeRTOS(教程非常详细）-优快云博客这是我认为比较详细的一个教程。

打开cubemx，找到freertos选项

然后在task中添加gm6020和dr16两个任务，任务参数如下：

其中要注意的是“Entry Function”这部分的名称要和代码里编写的函数名一样。

这里freertos代码处也别忘了定义。

第三部分：

遥控器的配置：

遥控器我们是通过DMA进行通信，其中C板上得链连接DBUS进行数据的接收。

波特率一般为100kb/s。我们打开C板用户手册，可以知道在C板的USART3上配置。

其中mode改为Circular

通过官方C板文件我们也可以编写遥控器的代码dr16.h和dr16.c

#ifndef __DR16_1_H__
#define __DR16_1_H__

#include "main.h"

typedef struct
{
	int16_t ch0;
	int16_t ch1;
	int16_t ch2;
	int16_t ch3;
	int8_t s1;
	int8_t s2;
	
}rc_info_t;
void RemoteDataProcess(rc_info_t*rc,uint8_t rx_data[18]);
extern rc_info_t dr16_receive;
#endif

void RemoteDataProcess(rc_info_t *rc,uint8_t rx_data[18])
{
	
	rc->ch0 = (((int16_t) rx_data[0] | (int16_t) rx_data[1] << 8) & 0x07FF)-1024;
  rc->ch1 = (((int16_t) rx_data[1] >> 3 | (int16_t) rx_data[2] << 5)  & 0x07FF)-1024;
	rc->ch2 = (((int16_t) rx_data[2] >> 6 | (int16_t) rx_data[3] | (int16_t) rx_data[4] << 10) & 0x7FF)-1024;
  rc->ch3 = (((int16_t) rx_data[4] >> 1 | (int16_t) rx_data[5] << 7 ) & 0x7FF)-1024;
  rc->s1  = ((int16_t) rx_data[5] >> 4) & 0x003;
  rc->s2 	= ((int16_t) rx_data[5] >> 6) & 0x003;
}

其中不要忘记在main.c中配置HAL_UART_Receive_DMA(&huart3,rx_buffer,sizeof(rx_buffer));以及在dma.c中配置extern rc_info_t dr16_receive;

遥控器配置电机跑起来：

之前我们提到，要想让电机的数据受到pid三个参数的控制，所以我们要初始化motor的pid

void motor_init(void){
    const float pid_const[3] = {50,0,0};//angle = 50 0 0,speed=19, 
    pid_init(&motor_data.motor_pid[1], pid_const[0],pid_const[1],pid_const[2],PID_MAX_OUTPUT ,PID_MAX_IOUT);
}

然后我们再配置motor_control函数（以角度环为例）：

void motor_control(void const * argument){
    motor_init();
    while(1){
        motor_data.angle_set=dr16_receive.s1*100;
        motor_data.angle=motor6020rx.angle;
        motor_data.give_current=pid_calc(&motor_data.motor_pid[1], motor_data.angle_set, motor_data.angle,8192);

        motor_write(0x1FF,motor_data.give_current,0,0,0);
        HAL_Delay(1);
				

    }


}

这里我们的设定值angle_set采用的是遥控器s1，其中s1的值是1、2、3

再通过pid_calc进行pid参数计算，后面的8192是指电机转过的最大角度。

最后通过向电机写入，来使电机转动。

最后通过debug，打开jscope观察波形，即可根据波形情况调节Kp、Ki和Kd

下面是效果展示：

速度环

角度环

复盘过程与感受：

        说实话本人第一次开始做这个任务的时候毫无头绪，我只是大致了解了我要实现什么效果，但是我不知道我该怎么实现？我试着将之前的工程组装在一起，比如遥控器的控制、6020电机的转动，但这些组装起来的庞然大物（一坨）能跑起来吗？编译后又爆了一万个错误，当我逐渐把这些错误逐一消除的时候仍然无济于事，我陷入迷茫陷入内耗因为我作为一个小白我只能死磕报错。我甚至没有养成找错误的一个习惯。我不知道是硬件出了问题接线出了问题还是代码前期配置出了问题。我认为这个经历是宝贵的，因为从这里我逐渐掌握自己解决问题的能力。后面我问了几个学长，也试着将我之前的工程推翻重新来过，我逐渐理清了思路，明白了各个函数的含义，知道了各个变量之间该如何产生联系。即便很多东西都是求助于他人，但我后面也逐渐去弄懂弄透背后的机理。

我也不是天赋哥，一看就懂。我只知道学习是循序渐进的过程，不能有任何捷径走，即便我读到优秀的开源我也不能框框照搬，我也得读懂并做到自己写出来，做到这里我也就觉得所学所做的是一件有意义的事。

最后这是我第一次这么完整的去搞懂这次任务，很感谢平时给我支持与帮助的学长，感谢他们对我的理解也感谢他们不厌其烦的回答我很糖的问题。最后整个工程文件我会打包放到我的github，之后有时间再用git传一下。愿我们无限进步！

后续应该做的：

1.减少电机的超调。

2.尝试用双闭环控制电机。

目前已知的错误：

1.debug界面无法接收到遥控器的数据

原因：波特率多了一个0（糖到不能再糖了吧）

复盘：当时一直执着于代码层面，但是代码中断进入也是正常，但就是一直死磕

2.遥控器无法向电机发送数据

原因：dma文件中一个变量未定义

复盘：前面知识掌握的不好，许多细节问题有遗漏

 

如果有大佬们路过，希望能给点建议！谢谢！