# RoboMaster电机上层驱动

**Repository Path**: ibst/robomaster-motor-upper-drive

## Basic Information

- **Project Name**: RoboMaster电机上层驱动
- **Description**: 北京交通大学 BNGU 电机2006,3508,6020上层驱动
- **Primary Language**: C
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 20
- **Forks**: 1
- **Created**: 2025-04-26
- **Last Updated**: 2025-12-23

## Categories & Tags

**Categories**: hardware

**Tags**: RoboMaster, DJI, Robot, BNGU, RM

## README

# Robomaster电机上层驱动

### 介绍

 _理解本驱动代码最好先理解电调如何通过CAN发电流值驱动电机_ 

Robomaster电机上层驱动,可以使用于M2006,M3508,GM6020
本开源代码基于大疆RoboMaster C板书写，移植代码注意引脚,时钟线等配置

#### 控制模式

总控制有2种,motor_ctrl_init,motor_MIT_ctrl_init只能二选一

@motor_ctrl_init控制方式是单独针对速度位置力矩分析或者进行元素叠加的控制

@motor_MIT_ctrl_init控制方式是只用控制torque+KP*position+KD*speed,本质为力矩控制

##### motor_ctrl_init

motor_ctrl_init模式：

```
#define Speed_Mode 0
#define Position_Mode 1
#define Speed_Position_Mode 2
#define Torque_Mode 3
#define Speed_Torque_Mode 4
#define Position_Torque_Mode 5
#define Speed_Position_Torque_Mode 6
#define Zero_Torque_Mode 7

#define DJI_M2006 8
#define DJI_M3508 9
#define DJI_M6020 10
```

motor_ctrl_init总使用函数（当然也可以使用单模式函数）：

```
void motor_3508_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t target_speed,uint16_t target_position,float force,uint8_t mode,uint8_t motor_type);
```

motor_ctrl_init单模式函数( 若想使用请加rm_motor_send_msg(&(motor->RM_CAN)); )：

```
void motor_3508_speed_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t speed);
void motor_3508_position_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t position);
void motor_3508_speed_position_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t speed,int32_t position);
void motor_3508_torque_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,float force,uint8_t motor_type);
void motor_3508_speed_torque_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t speed,float force,uint8_t motor_type);
void motor_3508_position_torque_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t speed,float force,uint8_t motor_type);
void motor_3508_speed_position_torque_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t speed,int32_t position,float force,uint8_t motor_type);
void motor_3508_zero_torque_ctrl(DJI_Motor_ControlTypeDef* motor,uint16_t id);
```

一个函数一次性控4个电机的单个模式函数
```
void speed_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t speed_1,int16_t speed_2,int16_t speed_3,int16_t speed_4,uint8_t motor_type);
void position_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t position_1,int16_t position_2,int16_t position_3,int16_t position_4,uint8_t motor_type);
void torque_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4,uint8_t motor_type);
void speed_torque_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t speed_1,int16_t speed_2,int16_t speed_3,int16_t speed_4,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4,uint8_t motor_type);
void position_torque_ctrl(DJI_Motor_ControlTypeDef* motor,int16_t position_1,int16_t position_2,int16_t position_3,int16_t position_4,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4,uint8_t motor_type); 
void zero_torque_ctrl(DJI_Motor_ControlTypeDef* motor);
```

##### motor_MIT_ctrl_init

MIT模式（极力推荐）

只用控制torque+KPxPosition+KDxSpeed,本质为力矩控制

```
void MIT_ctrl(DJI_Motor_ControlTypeDef* motor,uint8_t id,int16_t speed,int16_t position,float torque,uint8_t motor_type);
```

#### 控制原理

底层电机控制信号--CAN数据发送与接收函数
```
typedef struct __rm_motor_dataTypeDef
{
	uint16_t angle;
	int16_t speed;
	int16_t current;
	uint8_t temperature;
}rm_motor_dataTypeDef;

typedef struct __rm_motor_groupTypeDef
{
	uint16_t group_id;//ESC receive id
	CAN_HandleTypeDef *hcan;//The hcan used
	int16_t cmd[4];
	int16_t cmd_max[4];
	rm_motor_dataTypeDef motor_data[4];
}rm_motor_groupTypeDef;
```

电机控制信息
```
typedef struct 
{
	int16_t exp_speed;
	uint16_t exp_position;
	float exp_torque;	
}ExpTypeDef;

```

Speed_Torque_Mode使用的面向对象结构体
```
typedef struct
{
    uint16_t now_position; // 当前角度
    uint16_t last_position; // 上一次的位置
	uint16_t this_position;//走过的位置
	uint16_t last_target;//上一次的目标（防止反复执行）
	uint16_t stay_target;//维持目标位置
}motor_positionTypeDef;
```

上层数据Exp,当前position与PID参数

```
typedef struct 
{
	rm_motor_groupTypeDef CAN_3508;
    float cmd_buffer[4];
	ExpTypeDef M3508[4];
	pid_controller motor_3508_s_pid[4];
    pid_controller motor_3508_p_pid[4];
	motor_positionTypeDef motor_3508_position[4];
	uint8_t motor_speed_position_flag[4];
}DJI_Motor_ControlTypeDef;
```

注：rm_motor_groupTypeDef相关底层代码见jingyuan的rm_motor库

### 闭环示例

初始化代码

PS：2006电机无负载情况下，推荐速度KP=3，位置KP=0.215左右

```
//2006,3508 初始化
void motor_3508_ctrl_init(DJI_Motor_ControlTypeDef* motor)
{
	HAL_Delay(2000);
	rm_motor_init(&(motor->CAN_3508),0x200,&hcan1);
	rm_motor_cmd_max_config(&(motor->CAN_3508),4000,4000,4000,4000);
	for (int i = 0; i < 4; i++)
    {
        pid_controller_create(motor->motor_3508_s_pid + i, 3, 0, 0, 100, 16384);
		pid_controller_create(motor->motor_3508_p_pid + i, 0.22, 0, 0, 100, 16384);
    }
	motor_3508_pid_init(motor->motor_3508_s_pid,3,0,0,100);
	motor_3508_pid_init(motor->motor_3508_s_pid + 1,3,0,0,100);
	motor_3508_pid_init(motor->motor_3508_s_pid + 2,3,0,0,100);
	motor_3508_pid_init(motor->motor_3508_s_pid + 3,3,0,0,100);
	motor_3508_pid_init(motor->motor_3508_p_pid,0.22,0,0,100);
	motor_3508_pid_init(motor->motor_3508_p_pid + 1,0.22,0,0,100);
	motor_3508_pid_init(motor->motor_3508_p_pid + 2,0.22,0,0,100);
	motor_3508_pid_init(motor->motor_3508_p_pid + 3,0.22,0,0,100);
	rm_motor_cmd_refresh(&(motor->CAN_3508),0,0,0,0);
	rm_motor_send_msg(&(motor->CAN_3508));
}
```

速度环代码
```
void motor_3508_speed_pid(DJI_Motor_ControlTypeDef* motor,uint16_t id,int16_t target_speed)
{
	 float error=target_speed-motor->CAN_3508.motor_data[id].speed;
	 pid_error_input(motor->motor_3508_s_pid+id,error);
	 motor->cmd_buffer[id]=pid_output(motor->motor_p_pid+id);
	 
	 //M2006无温度回传 3508 6020 可用
	 if(motor->CAN_3508.motor_data[id].temperature>=50)
	 {
	   motor->cmd_buffer[id]=0;
	 } 

}
```

对ID为1的3508电机给RPM540的速度

```
DJI_Motor_ControlTypeDef M3508_Motor;
bsp_can_init();
motor_3508_ctrl_init(&M3508_Motor);
bsp_tim_init();

void dial_execute()
{
     motor_3508_ctrl(&M3508_Motor,0,540,0,0,Speed_Mode,DJI_M3508);
}
```


### 大疆电机文档
M2006/M3508

![输入图片说明](M2006,3508.png)

GM6020

![输入图片说明](M6020.png)

### 更新日志
#### 2025.4.26 
创建代码demo

#### 2025.4.28 
封装pid_controller，motor_positionTypeDef，motor_spped_position_flag等对象，扩展如下高封装函数：

```
//一个函数一次性控4个电机单个模式
void speed_ctrl(rm_motor_groupTypeDef* motor,int16_t speed_1,int16_t speed_2,int16_t speed_3,int16_t speed_4);
void position_ctrl(rm_motor_groupTypeDef* motor,int16_t position_1,int16_t position_2,int16_t position_3,int16_t position_4);
void torque_ctrl(rm_motor_groupTypeDef* motor,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4);
void speed_torque_ctrl(rm_motor_groupTypeDef* motor,int16_t speed_1,int16_t speed_2,int16_t speed_3,int16_t speed_4,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4);
void position_torque_ctrl(rm_motor_groupTypeDef* motor,int16_t position_1,int16_t position_2,int16_t position_3,int16_t position_4,int16_t torque_1,int16_t torque_2,int16_t torque_3,int16_t torque_4); 

```

#### 2025.5.1 
![输入图片说明](%E5%BC%80%E5%8F%91%E6%97%A5%E5%B8%B8.jpeg)
高铁上对代码的编写与优化

重构代码，封装rm_motor_groupTypeDef，完善高封装，驱动文件增加可移植性，新增零力矩模式，规范命名，增加GM6020驱动

DJI_Motor_ControlTypeDef总结构体：
```
typedef struct 
{
	int16_t exp_speed;
	uint16_t exp_position;
	float exp_torque;	
}ExpTypeDef;

typedef struct
{
    uint16_t now_position; // 当前角度
    uint16_t last_position; // 上一次的位置
	uint16_t this_position;//走过的位置
	uint16_t last_target;//上一次的目标（防止反复执行）
	uint16_t stay_target;//维持目标位置
}motor_positionTypeDef;

typedef struct 
{
	rm_motor_groupTypeDef CAN_3508;
	ExpTypeDef M3508[4];
	pid_controller motor_3508_s_pid[4];
    pid_controller motor_3508_p_pid[4];
	motor_positionTypeDef motor_3508_position[4];
	uint8_t motor_speed_position_flag[4];
}DJI_Motor_ControlTypeDef； 

```

#### 2025.5.2
新增MIT控制模式

```
//MIT_ctrl时,只用控制torque+KP*position+KD*speed,
void MIT_ctrl(DJI_Motor_ControlTypeDef* motor,uint8_t id,int16_t speed,int16_t position,float torque,uint8_t motor_type)
{
	motor_3508_position_pid(motor,id,position);
    motor_3508_torque_cal(motor,id,torque,motor_type);
	float error = speed - motor->CAN_3508.motor_data[id].speed;
	pid_error_input(motor->motor_3508_s_pid+id,error);
	motor->CAN_3508.cmd[id] += pid_output(motor->motor_3508_s_pid+id);
	rm_motor_cmd_refresh(&(motor->CAN_3508),motor->CAN_3508.cmd[0],motor->CAN_3508.cmd[1],motor->CAN_3508.cmd[2],motor->CAN_3508.cmd[3]);
    rm_motor_send_msg(&(motor->CAN_3508));
}
```

#### 2025.5.3
优化命名

#### 2025.5.4
优化带宽：避免每个电机控制都发一次报

#### 2025.5.8
避免竞争：加入float cmd_buffer[4];



```
rm_motor_cmd_refresh(&(motor->RM_CAN),motor->RM_CAN.cmd[0],motor->RM_CAN.cmd[1],motor->RM_CAN.cmd[2],motor->RM_CAN.cmd[3]);
```
修改为：


```
rm_motor_cmd_refresh(&(motor->RM_CAN),motor->cmd_buffer[0],motor->cmd_buffer[1],motor->cmd_buffer[2],motor->cmd_buffer[3]);
```

#### 2025.5.14
增加一些示例

#### 2025.5.15
PID对象根据不同的电机拆分
```
void motor_pid_data_init(DJI_Motor_ControlTypeDef* motor,uint8_t motor_type,uint8_t control_type)
{
	if(control_type==Nom_Ctrl)
	{
	 if(motor_type==8||motor_type==9)
	 {
	    motor_pid_init(motor->motor_s_pid,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 1,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 2,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 3,3,0,0,100);
	    motor_pid_init(motor->motor_p_pid,0.22,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 1,0.22,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 2,0.22,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 3,0.22,0,0,100);
	 }
	 else if(motor_type==10)
	 {
	    motor_pid_init(motor->motor_s_pid,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 1,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 2,3,0,0,100);
	    motor_pid_init(motor->motor_s_pid + 3,3,0,0,100);
	    motor_pid_init(motor->motor_p_pid,0.22,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 1,8,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 2,0.22,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 3,0.22,0,0,100);
	   }
    }
	if(control_type==MIT_Ctrl)
	{
		motor_pid_init(motor->motor_s_pid,0,0,0.2,100);
	    motor_pid_init(motor->motor_s_pid + 1,0,0,0.2,100);
	    motor_pid_init(motor->motor_s_pid + 2,0,0,0.2,100);
	    motor_pid_init(motor->motor_s_pid + 3,0,0,0.2,100);
	    motor_pid_init(motor->motor_p_pid,4,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 1,2,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 2,2,0,0,100);
	    motor_pid_init(motor->motor_p_pid + 3,2,0,0,100);
	}
}
```
#### 2025.5.16
VScode风美观代码

#### 2025.5.22
位置环error由4096改为8191/2
```
float error=target_position-motor->RM_CAN.motor_data[id].angle;	 
	 if(error>8191/2)
	 {
	     error-=8191;
     }
	 else if(error<-8191/2)
	 {
	     error+=8191;
     }
```
M3508.h内置控制结构体

```
extern DJI_Motor_ControlTypeDef M2006_Motor;
extern DJI_Motor_ControlTypeDef M3508_Motor;
extern DJI_Motor_ControlTypeDef M6020_Motor;
```
加入总线选择

#### 2025.5.23
在我们使用云台电机控制的时候，会出现两个电机分别由MIT与速度环控制，故开放MIT更新接口

```
//MIT控制,一个函数控单个电机多种模式(推荐)
void MIT_ctrl(DJI_Motor_ControlTypeDef* motor, uint8_t id, int16_t speed, int16_t position, float torque, uint8_t motor_type);
//使用MIT_ctrl请加上:
//rm_motor_cmd_refresh(&(motor->RM_CAN), motor->cmd_buffer[0], motor->cmd_buffer[1], motor->cmd_buffer[2], motor->cmd_buffer[3]);
//rm_motor_send_msg(&(motor->RM_CAN));
```

#### 2025.5.28
修改了三个电机的力矩系数

#### 2025.6.1
修补问题

#### 2025.6.11
做梦想到可以define优化

```
#define PID_CAL_RATE 1000
#define PID_MAX 8000
```

#### 2025.6.19

motor_3508_speed_torque_ctrl模式重构

#### 2025.7.15

电机位置累积功能

![输入图片说明](%E7%94%B5%E6%9C%BA%E4%BD%8D%E7%BD%AE%E7%B4%AF%E7%A7%AF.png)

#### 2025.11.24

优化PID算法,积分与微分的鲁棒性大大提升

### 参考代码

jingyuan写的rm_motor以及pid库

https://gitee.com/jingyuancode/rm_motor