如何用Robomaster C610/820R电调组成伺服电机控制系统（附STM32F407源码实现及分析）

本文链接：https://blog.youkuaiyun.com/Introver_t/article/details/80103041

写在前面，因为代码在一个工程里很混乱，难以单独剥离出来，所以求源码的我就不一一回复发送了，请大家见谅。但可以参考Robomaster官方提供的DEMO，官方的代码条理性更强，更适合拿来参考，传送门：https://rm-static.djicdn.com/tem/16856/RoboMaster%20M2006%20%E5%8A%A8%E5%8A%9B%E7%B3%BB%E7%BB%9F%20DEMO%20%E7%A8%8B%E5%BA%8F%E6%BA%90%E7%A0%81.7z

---------------------------------------------------------------- 正文开始 ----------------------------------------------------------------

前段时间入手了大疆为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 <stm32f4xx.h>
#include <stdlib.h>
#include <string.h>

#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  Fu