如何用Robomaster C610/820R电调组成伺服电机控制系统？

写在前面 ，因为代码在一个工程里很混乱，难以单独剥离出来，所以求源码的我就不一一回复发送了，请大家见谅。但可以参考Robomaster官方提供的DEMO，官方的代码条理性更强，更适合拿来参考
前段时间入手了大疆为Robomaster比赛推出的一套输出系统，C610无刷直流电机调速器以及M2006 P36直流无刷减速电机。首先简单讲解一下伺服电机控制系统的基本常识，随后会贴出源代码并结合代码分析控制原理。
伺服电机控制系统三环控制

一、电流环：
根据无刷电机的原理，其转动力矩和电机输入的转子电流成正比。因此对电机的电流闭环，实际上就是对电机输出的转动力矩实现精确的闭环控制，同时电流环也是伺服电机三环控制中最底层或者说是最内层的一环。不过电流环实际上不涉及到用户的编程实现，因为电流的闭环正是电调的工作，即C610已经帮我们做好了底层的电流闭环。当然了，对物理稍了解一些的也都知道，力和加速度成正比，其商为物体的质量，同样的，力矩与物体转动的角加速度也成正比，其商为系统的转动惯量。因此，我个人对电流环的理解其实更倾向于将其认为是电机的加速度环。
二、速度环：
速度环是嵌套在电流环外的一个闭环，该环的控制对象是电机转速，电机的转速通常是由编码器的反馈经过运算得来的，其输出的控制量为电流值。速度环可理解为通过调节电流达到反馈控制电机转速的闭环。通常速度环使用的反馈控制算法就是简单的PID，在本篇博客中使用的是PD控制，对C610来讲已经能有比较好的效果。
三、位置环：
位置环是伺服电机三环中最外层的一环，其控制对象是电机的位置即编码器数值，输出的控制量为预期转速。位置环的反馈方式有很多种，其中最简单的是直接采用PID算出当前期望的转速。当然，还可以使用前馈来规划未来一段时间内电机每个时刻需要到达的转速，同时将PID复合进来实现闭环。本博客中附带的代码使用的是最简单的PID闭环。
其实不难看出，伺服电机的三环：加速度、速度、位置，从内到外实际上是两次对时间的积分，通过控制加速度来达到预期的转速，通过控制转速来达到预期的位置。同时从闭环算法的角度来看，我们在软件上实现的其实是串级PID，外级是对位置的闭环，内级是对转速的闭环。


源代码分析

首先，明确一下C610电调的驱动方式，这些实际上在C610电调使用说明书上都有。










头文件：Include





#ifndef _820R_H_
#define _820R_H_
/**
******************************************************************************
* @file : 820R.h
* @brief : RM820R Driver's header file
* @author : Cui
* @date : 2018/3/22
******************************************************************************
* DJI Robomasters RM820R Motor governor Driver, using STM32F407VGT6 Std
* Peripherals DriversLib.
*/

/* Includes ------------------------------------------------------------------*/
#include
#include
#include

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"

#include "usart.h"
#include "cuiPid.h"

#include "can2.h"
包含了一份自己写的CAN总线驱动、串口外设驱动、PID实现，以及FreeRTOS的头文件。





头文件：宏、结构体


/* Macro definitions ---------------------------------------------------------*/
#define MOTOR_ID 0x201 ///< 电调ID
#define CNT_PER_ROUND 8192 ///< 外转子转一圈的脉冲数
#define REDUCTION_RATIO 36 ///< 减速比
#define MOTOR_TOLERANCE 8192 ///< 位置环容差
#define CNT_PER_ROUND_OUT (CNT_PER_ROUND * REDUCTION_RATIO) ///< 电机轴转一圈的脉冲数

/* Struct or Class definitions -----------------------------------------------*/
typedef struct{
int16_t Current;
int16_t Angle;
int16_t Speed;
int32_t CNT_ABS;
}
RM820R_TypeDef;

/* Extern global variables ---------------------------------------------------*/
extern SemaphoreHandle_t RM820R_RX_SEM;
extern RM820R_TypeDef RM820R[4];
extern int32_t RM820R_Target;
电机的主要参数用宏定义定义出来，方便后续修改。





头文件：函数声明


/* Function declarations -----------------------------------------------------*/

/**
* @brief Function to Initialize params for Position loop PID
* @param pid : pointer of PPM Pid struct
* @retval None
*/
void RM820RPPM_pid_init(pid_t* pid);

/**
* @brief RM820R Can Init Function
* @param CANx : CAN Num which mounts the RM820R
* @retval None
*/
void RM_CAN_Init(CAN_TypeDef* CANx);

/**
* @brief Independent Can message transmit function used by RM820R
* @param msg : pointer of msg buf
* len : length of msg buf
* id : CAN ID of Target RM820R
* @retval None
*/
u8 RM_CAN_Send(u8* msg,u8 len,int16_t id);

/**
* @brief Set all four RM820Rs' Current
* @param c1 : Current for RM820R ID 1
* @retval None
*/
void RM_Set_All_Current(int16_t c1, int16_t c2, int16_t c3, int16_t c4);

/**
* @brief Set one selected RM820R's Current
* @param Current : Current for selected RM820R
* @retval -1 : Using error
* 0 : Right
*/
int RM820R_SetCurrentSingle(uint8_t ID,int16_t Current);

/**
* @brief Function to exchange MSB and LSB
* @param pData : Target data pointer
* @retval None
*/
void exchange_HL(uint8_t* pData);

/**
* @brief Function to update feedback from CAN Bus to Varibles RM820R[]
* @param None
* @retval None
*/
void RM820R_GetFeedBack(void);

/**
* @brief Function to Set target Speed.
* @param None
* @retval None
*/
void RM820R_PVM(int32_t spd);
/**
* @brief Function to Set target Position.
* @param None
* @retval None
*/
void RM820R_PPM(int32_t cnt);
/**
* @brief Unit conversion Function
* @param ang : cnt of Motor
* @retval angle of Motor
*/
double RM820R_Cnt2Ang(int32_t cnt);
/**
* @brief Thread/Task Function in FreeRTOS to handle The control of RM820R
* @param pvParameters : None
* @retval None
*/
void vRM820R_PPMTask(void* pvParameters);
void vRM820R_PVMTask(void* pvParameters);



除了CAN总线的驱动函数以及角度换算函数外，最后两个Task函数是我们在驱动电机时，需要创建的任务，这两个任务就是真正执行速度闭环与位置闭环的任务。














源文件：全局变量


/**
******************************************************************************
* @file : 820R.c
* @brief : RM820R Driver program body
* @author : Cui
* @date : 2018/3/22
******************************************************************************
* DJI Robomasters RM820R Motor governor Driver, using STM32F407VGT6 Std
* Peripherals DriversLib.
*/

/* Includes ------------------------------------------------------------------*/
#include "820R.h"

/* Macro definitions ---------------------------------------------------------*/

/* Extern global variables ---------------------------------------------------*/
extern CanRxMsg RxMessage;

extern int16_t cnt_buf;
extern int16_t spd_buf;

extern TaskHandle_t RM820R_PVM_TaskHandle;
extern TaskHandle_t RM820R_PPM_TaskHandle;
/* Global variables ----------------------------------------------------------*/
SemaphoreHandle_t RM820R_RX_SEM;

RM820R_TypeDef RM820R[4];

int32_t RM820R_Target = 5000;
int32_t RM802R_SpdTarget = 0;
int32_t RM820R_CntBase = 0;

CAN_TypeDef* RM_CAN_Num;
u8 RM_mbox_queue[3] = { 0 };
u8 RM_mbox_index = 0;
extern全局变量，一部分来自CAN2接收中断，用于保存电调发来的回文数据，另一部分则用于FreeRTOS的任务调度，于main.c中定义，方便电机速度模式与位置模式的切换。


文件中的全局变量，分别用于保存电机当前的速度目标以及位置目标，还有CAN收发的处理。





源文件：函数原型


/**
* @brief RM820R Can Init Function
* @param CANx : CAN Num which mounts the RM820R
* @retval None
*/
void RM_CAN_Init( CAN_TypeDef* CANx )
{
RM_CAN_Num = CANx;
}
由于CAN总线初始化调用了于其他文件实现的函数，因此该函数只负责记录电机发送指令时需要调用CAN1还是CAN2。


/**
* @brief Independent Can message transmit function used by RM820R
* @param msg : pointer of msg buf
* len : length of msg buf
* id : CAN ID of Target RM820R
* @retval None
*/
u8 RM_CAN_Send( u8* msg, u8 len, int16_t id ){
u8 mbox;
/* u16 i=0; */
CanTxMsg TxMessage;
TxMessage.StdId = id; /* StdID 11Bits */
TxMessage.IDE = CAN_Id_Standard; /* 使用标准标识符 */
TxMessage.RTR = CAN_RTR_Data; /* 发送数据帧 */
TxMessage.DLC = len; /* 数据长度 */

memcpy( &TxMessage.Data[0], &msg[0], len * sizeof(u8) );

mbox = CAN_Transmit(RM_CAN_Num, &TxMessage); /* 获取邮箱号 */
RM_mbox_queue[RM_mbox_index] = mbox;
RM_mbox_index++;
if ( RM_mbox_index > 2 ){
RM_mbox_index = 0;
}
return(0);
}
CAN发送函数。


/**
* @brief Set all four RM820Rs' Current
* @param c1 : Current for RM820R ID 1
* @retval None
*/
void RM_Set_All_Current( int16_t c1, int16_t c2, int16_t c3, int16_t c4 )
{
u8 Buffer[8];
memcpy( &Buffer[0], &c1, 2 );
memcpy( &Buffer[2], &c2, 2 );
memcpy( &Buffer[4], &c3, 2 );
memcpy( &Buffer[6], &c4, 2 );

exchange_HL( &Buffer[0] );
exchange_HL( &Buffer[2] );
exchange_HL( &Buffer[4] );
exchange_HL( &Buffer[6] );

RM820R[0].Current = c1;
RM820R[1].Current = c2;
RM820R[2].Current = c3;
RM820R[3].Current = c4;

RM_CAN_Send( Buffer, 8, 0x200 );
}

/**
* @brief Set one selected RM820R's Current
* @param Current : Current for selected RM820R
* @retval -1 : Using error
* 0 : Right
*/
int RM820R_SetCurrentSingle( uint8_t ID, int16_t Current )
{
uint8_t Buffer[8] = { 0 };

if ( ID < 1 || ID > 4 )
{
return(-1);
}

switch ( ID )
{
case 1:
RM820R[0].Current = Current;
memcpy( &Buffer[0], &Current, 2 );
exchange_HL( &Buffer[0] );
break;
case 2:
RM820R[1].Current = Current;
memcpy( &Buffer[2], &Current, 2 );
exchange_HL( &Buffer[2] );
break;
case 3:
RM820R[2].Current = Current;
memcpy( &Buffer[4], &Current, 2 );
exchange_HL( &Buffer[4] );
break;
case 4:
RM820R[3].Current = Current;
memcpy( &Buffer[6], &Current, 2 );
exchange_HL( &Buffer[6] );
break;
}
RM_CAN_Send( Buffer, 8, 0x200 );
return(0);
}
前一个为一次性设定四个电机的电流值，后一个为设定指定电机的电流值。实际上就是实现了C610说明书中的CAN数据发送协议。





/**
* @brief Function to exchange MSB and LSB
* @param pData : Target data pointer
* @retval None
*/
void exchange_HL( uint8_t* pData ){
uint8_t data_exBuf;

data_exBuf = pData[0];
pData[0] = pData[1];
pData[1] = data_exBuf;
}
需要注意的是，接收报文和反馈报文中的数据，都是高八位在前，第八位在后，这与一些人的习惯不同，一开始免不了踩坑。此函数是将数据的高八位与第八位互换，方便后续拷贝内存。


/**
* @brief Function to update feedback from CAN Bus to Varibles RM820R[]
* @param None
* @retval None
*/
void RM820R_GetFeedBack( void )
{
exchange_HL( &(RxMessage.Data[0]) );
exchange_HL( &(RxMessage.Data[2]) );

memcpy( &cnt_buf, &(RxMessage.Data[0]), 2 );
memcpy( &spd_buf, &(RxMessage.Data[2]), 2 );

switch ( RxMessage.StdId )
{
case 0x201:
RM820R[0].Angle = cnt_buf;
RM820R[0].Speed = spd_buf;
break;
case 0x202:
RM820R[1].Angle = cnt_buf;
RM820R[1].Speed = spd_buf;
break;
case 0x203:
RM820R[2].Angle = cnt_buf;
RM820R[2].Speed = spd_buf;
break;
case 0x204:
RM820R[3].Angle = cnt_buf;
RM820R[3].Speed = spd_buf;
break;
}
}
将CAN总线上接收到的电调返回的原始编码器数值与速度写入结构体变量中。注意这里并没有检测编码器的数值跳变，原因是电调默认的回文频率是1kHz，1ms就能接收到一次电调发给主控的回文，但我控制任务的周期是10ms，如果在控制任务中检测编码器数值的溢出，很容易丢失数据，因此我的溢出检测放在CAN接收中断中实现，后续会贴出这部分代码。


/**
* @brief Function to Initialize params for Position loop PID
* @param pid : pointer of PPM Pid struct
* @retval None
*/
void RM820RPVM_pid_init( pid_t* pid )
{
pid->Kp = 2.3;
pid->Ki = 0;
pid->Kd = 2.0;

pid->Iteg_max = 1000;
pid->Max_out = 10000; /* /< Current */
pid->Min_out = -10000; /* /< Current */

pid->err[0] = 0;
pid->err[1] = 0;
pid->real_value[0] = 0;
pid->real_value[1] = 0;

pid->target_value = 0;

pid->death_zone = 20;
}

void RM820RPPM_pid_init( pid_t* pid )
{
pid->Kp = 0.22;
pid->Ki = 0.01;
pid->Kd = 0.35;

pid->Iteg_max = 500;
pid->Max_out = 3000; /* /< Current */
pid->Min_out = -3000; /* /< Current */

pid->err[0] = 0;
pid->err[1] = 0;
pid->real_value[0] = 0;
pid->real_value[1] = 0;

pid->target_value = 0;

pid->death_zone = 350;
}
以上两个是用于初始化闭环PID参数的函数，PVM是速度环的PID参数初始化，PPM是位置环的PID参数初始化，pid_t是自己实现的pid算法结构体类型。


/**
* @brief Unit conversion Function
* @param ang : angle of shaft
* @retval cnt of rotor
*/
int32_t RM820R_Ang2Cnt(int ang)
{
int32_t cnt = (CNT_PER_ROUND_OUT * ang) / 360.0 + RM820R_CntBase;
return(cnt);
}



/**
* @brief Unit conversion Function
* @param ang : cnt of shaft
* @retval angle of rotor
*/
double RM820R_Cnt2Ang(int32_t cnt){
return (double)(((cnt - RM820R_CntBase) * 360.0)/CNT_PER_ROUND_OUT);
}
单位换算函数，如电机参数发生变化，只需修改上面提到的宏定义即可。


void RM820R_PVM(int32_t spd){
RM802R_SpdTarget = spd;
vTaskSuspend(RM820R_PPM_TaskHandle);
}

void RM820R_PPM(int32_t cnt){
RM820R_Target = cnt + RM820R_CntBase;
vTaskResume(RM820R_PPM_TaskHandle);
}
对外调用的设置电机转速或位置的函数，注意这里涉及到任务的挂起与解挂，因为如果需要电机直接以速度模式运行，就必须停止掉位置环的运算，因为位置环内部实际上一直在设定当前的速度目标值并由速度环去执行，这样会干扰到速度环的运算。


/**
* @brief Stop sending speed instruction to RM820R
* @param None
* @retval None
*/
void RM820R_StopWorking( void )
{
vTaskSuspend( RM820R_PVM_TaskHandle );
vTaskSuspend( RM820R_PVM_TaskHandle );
}
电机停止工作，这个函数比较危险，因为电调默认会执行上次接收到的电流命令，调用这个函数之后，电机不会立刻停转，直到触发电调的超时保护，才会自己断电。因此，这个函数的用途只是在确保电机已经停转之后，关闭掉速度环和位置环。


void vRM820R_PVMTask(void* pvParameters){
BaseType_t CAN_RX = pdFALSE;
int16_t cur_buf = 0;
pid_t RM820R_PVM_PID;

RM_CAN_Init(CAN2);
Can_Init(CAN2);

RM820RPVM_pid_init(&RM820R_PVM_PID);

while(1){
pid_setTarget( &RM820R_PVM_PID, RM802R_SpdTarget );
CAN_RX = xSemaphoreTake( RM820R_RX_SEM, 0 );
if ( CAN_RX == pdTRUE )
{
CAN_RX = pdFALSE;
RM820R_GetFeedBack();
}
cur_buf = pid_calculate(&RM820R_PVM_PID,RM820R[0].Speed);
RM820R_SetCurrentSingle(1,cur_buf);

vTaskDelay(10);
}///< end of task while(1)

}
这里就是速度环的真身，并调用了真正的CAN总线初始化。死循环中的逻辑很明显，设定当前的速度目标值，检测CAN总线上的数据是否有更新，如果更新，则读取当前速度，并计算当前需要给上的电流值。


/**
* @brief Thread/Task Function in FreeRTOS to handle The control of RM820R
* @param pvParameters : None
* @retval None
*/
void vRM820R_Task( void* pvParameters )
{
pid_t RM820R_PPM_PID;
int32_t spd_buf;
BaseType_t CAN_RX = pdFALSE;

RM820RPPM_pid_init( &RM820R_PPM_PID );

while(1){
CAN_RX = xSemaphoreTake( RM820R_RX_SEM, 0 );
if (CAN_RX == pdTRUE){
CAN_RX = pdFALSE;
RM820R_GetFeedBack();
RM820R_CntBase = RM820R[0].CNT_ABS;
printf("RM820R Init Done. CntBase = %d.rn",RM820R_CntBase);
break;
}
vTaskDelay(10);
}
RM820R_Set_Ang(180);

while(1){
pid_setTarget(&RM820R_PPM_PID,RM820R_Target);
spd_buf = pid_calculate(&RM820R_PPM_PID,RM820R[0].CNT_ABS);
RM820R_Set_Spd(spd_buf);
vTaskDelay(10);
}
}
这里，本身位置闭环很简单，和速度环类似，但需要注意第一个while中，是上电后对电机编码器初始值进行采样，因为实测发现这个电机上电之后的编码器初始值是不能确定的，比如一次上电的初始值是3400，下一次上电就可能是4000。





最后，贴出CAN总线初始化以及CAN中断处理函数。





void Can_Init( CAN_TypeDef* CANx ){
GPIO_InitTypeDef Gpio_Structure;
CAN_InitTypeDef Can_Structure;
NVIC_InitTypeDef NVIC_InitStructure;
CAN_FilterInitTypeDef Can_filter_Structure;

CAN_Num = CANx;

if ( CANx == CAN1 )
{
RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOA, ENABLE );
RCC_APB1PeriphClockCmd( RCC_APB1Periph_CAN1, ENABLE );

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

Gpio_Structure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12;
Gpio_Structure.GPIO_Mode = GPIO_Mode_AF;
GPIO_Init( GPIOA, &Gpio_Structure );
}else if( CANx == CAN2 )
{
RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOB, ENABLE );
RCC_APB1PeriphClockCmd( RCC_APB1Periph_CAN1 | RCC_APB1Periph_CAN2, ENABLE );

GPIO_PinAFConfig( GPIOB, GPIO_PinSource12, GPIO_AF_CAN2 );
GPIO_PinAFConfig( GPIOB, GPIO_PinSource13, GPIO_AF_CAN2 );

Gpio_Structure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13;
Gpio_Structure.GPIO_Mode = GPIO_Mode_AF;
GPIO_Init( GPIOB, &Gpio_Structure );
}

if ( CANx == CAN2 )
{
NVIC_InitStructure.NVIC_IRQChannel = CAN2_RX0_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5;
}else {
NVIC_InitStructure.NVIC_IRQChannel = CAN1_RX0_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 6;
}

NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init( &NVIC_InitStructure );

CAN_DeInit( CANx );
CAN_StructInit( &Can_Structure );
Can_Structure.CAN_TTCM = DISABLE;
Can_Structure.CAN_ABOM = ENABLE;
Can_Structure.CAN_AWUM = DISABLE;
Can_Structure.CAN_NART = DISABLE;
Can_Structure.CAN_RFLM = DISABLE;
Can_Structure.CAN_TXFP = ENABLE;
Can_Structure.CAN_Mode = CAN_Mode_Normal;
Can_Structure.CAN_SJW = CAN_SJW_1tq;
Can_Structure.CAN_BS1 = CAN_BS1_9tq;
Can_Structure.CAN_BS2 = CAN_BS2_4tq;
Can_Structure.CAN_Prescaler = 3;
CAN_Init( CANx, &Can_Structure );

Can_filter_Structure.CAN_FilterNumber = 14;
Can_filter_Structure.CAN_FilterMode = CAN_FilterMode_IdList;
Can_filter_Structure.CAN_FilterScale = CAN_FilterScale_16bit;
Can_filter_Structure.CAN_FilterIdHigh = ((uint16_t)0x71 << 5);
Can_filter_Structure.CAN_FilterIdLow = ((uint16_t)0x72 << 5);
Can_filter_Structure.CAN_FilterMaskIdHigh = ((uint16_t)0x201 << 5);
Can_filter_Structure.CAN_FilterMaskIdLow = ((uint16_t)0x200 << 5);
Can_filter_Structure.CAN_FilterFIFOAssignment = 0;
Can_filter_Structure.CAN_FilterActivation = ENABLE;
CAN_FilterInit( &Can_filter_Structure );


CAN_ITConfig( CANx, CAN_IT_FMP0, ENABLE );
}
这里的CAN滤波器使用了标识符列表模式,只接收指定的四个ID，实测发现过于频繁的进入中断会导致各种CAN总线上的错误。


void CAN2_RX0_IRQHandler( void )
{
static int32_t RM820R_ori = 0;

static int32_t RM820Rtmp;
static int32_t RM820R_base = 0;
static int32_t RM820Rtmp_pre = 0;
BaseType_t HigherTaskWoken;
if ( CAN_GetITStatus( CAN2, CAN_IT_FMP0 ) != RESET )
{
CAN_Receive( CAN2, CAN_FIFO0, &RxMessage );

/* Message From RM820R -------------------------------------------------------*/
if ( RxMessage.StdId == 0x201 ){
exchange_HL( &(RxMessage.Data[0]) );
memcpy( &cnt_buf, &(RxMessage.Data[0]), 2 );
RM820Rtmp = (int16_t) (cnt_buf);
if ( RM820Rtmp - RM820Rtmp_pre > 4095 ){
RM820R_base -= 8191;
}
else if ( RM820Rtmp - RM820Rtmp_pre < -4095 ){
RM820R_base += 8191;
}
RM820Rtmp_pre = RM820Rtmp;
RM820R_ori = RM820R_base + RM820Rtmp;
RM820R[0].CNT_ABS = RM820R_ori;
xSemaphoreGiveFromISR( RM820R_RX_SEM, &HigherTaskWoken );
}
}
}
CAN接收中断。