基于 RT-Thread 的 RoboMaster 电控框架（一）

背景

电机模块开发

使用电机和电调均为大疆官方出品，如 2006，3508，6020 等，采用 CAN 通讯方式。

构建对象

首先我们根据使用的电机特性，构建一个通用的电机对象

 1/**
 2 * @brief DJI intelligent motor typedef
 3 */
 4typedef struct dji_motor_object
 5{
 6    rt_device_t  can_dev;                   // 电机CAN实例
 7    dji_motor_measure_t measure;            // 电机测量值
 8    uint32_t tx_id;                         // 发送id(主发)
 9    uint32_t rx_id;                         // 接收id(主收)
10    /* 分组发送设置 */
11    uint8_t send_group;                     // 同一帧报文分组
12    uint8_t message_num;                    // 一帧报文中位置
13    motor_type_e motor_type;                // 电机类型
14    motor_working_type_e stop_flag;         // 启停标志
15    /* 监控线程相关 */
16    rt_timer_t timer;                       // 电机监控定时器
17    /* 电机控制相关 */
18    void *controller;            // 电机控制器
19    int16_t (*control)(dji_motor_measure_t measure);   // 控制电机的接口 用户可以自定义,返回值为16位的电压或电流值
20} dji_motor_object_t;

因为这些电机我们均使用 CAN 方式进行驱动，是 CAN 设备的延申，于是将 rt_device_t can_dev 父类结构体对象内嵌。

dji_motor_measure_t 结构体中为，电机控制时需要用到的一些反馈值，包括电调直接反馈的数据以及进一步解算的得出的：

 1/**
 2 * @brief DJI motor feedback
 3 */
 4typedef struct
 5{
 6    /* 以下是处理得出的数据 */
 7    float angle_single_round; // 单圈角度
 8    float speed_aps;          // 角速度,单位为:度/秒
 9    float total_angle;        // 总角度,注意方向
10    int32_t total_round;      // 总圈数,注意方向
11    float  target;            // 目标值(输出轴扭矩矩/速度/角度(单位度))
12    /* 以下是电调直接回传的数据 */
13    uint16_t ecd;             // 0-8191
14    uint16_t last_ecd;        // 上一次读取的编码器值
15    int16_t  speed_rpm;       //电机的转速值
16    int16_t real_current;     // 实际转矩电流
17    uint8_t temperature;      // Celsius
18} dji_motor_measure_t;

注册实例

通过 dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller) 注册对应的电机实例，用户通过 motor_config_t *config 对实例进行灵活配置：

 1/**
 2 * @brief 电机初始化,返回一个电机实例
 3 * @param config 电机配置
 4 * @return dji_motor_object_t* 电机实例指针
 5 */
 6dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller)
 7{
 8    dji_motor_object_t *object = (dji_motor_object_t *)rt_malloc(sizeof(dji_motor_object_t));
 9    rt_memset(object, 0, sizeof(dji_motor_object_t));
10    // 对接用户配置的 motor_config
11    object->motor_type = config->motor_type;                         // 6020 or 2006 or 3508
12    object->rx_id = config->rx_id;                                   // 电机接收报文的ID
13    object->control = controller;                                    // 电机控制器
14    /* 查找 CAN 设备 */
15    object->can_dev = rt_device_find(config->can_name);
16    // 电机分组,因为至多4个电机可以共用一帧CAN控制报文
17    motor_send_grouping(object, config);
18    // 电机离线检测定时器相关
19    object->timer = rt_timer_create("motor1",
20                             motor_lost_callback,
21                             object, 20,
22                             RT_TIMER_FLAG_PERIODIC);
23    rt_timer_start(object->timer);
24    dji_motor_enable(object);
25    dji_motor_obj[idx++] = object;
26    return object;
27}
28/* 电机配置结构体 */
29typedef struct
30{
31    motor_type_e motor_type;
32    const char *can_name;
33    uint32_t tx_id;                // 发送id(主发)
34    uint32_t rx_id;                // 接收id(主收)
35    void *controller;        // 电机控制器
36} motor_config_t;

1static struct chassis_controller_t{
2    pid_object_t *speed_pid;
3}chassis_controller;
4static struct gimbal_controller_t{
5    pid_object_t *speed_pid;
6    pid_object_t *angle_pid;
7}gimbal_controlelr;

调用 dji_motor_object_t *dji_motor_register 时传入的 void *controller 为电机对应的控制器具体实现，如进行 pid 计算，滤波等，会赋值给电机对象对应的函数指针，在进行电机控制计算时被执行，如下：

1rt_int16_t chassis_control(dji_motor_measure_t measure){
2    static rt_int16_t set = 0;
3    set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
4    return set;
5}

数据处理

电机对象离不开对数据稳定快速的收发和解析计算，接下来展开讨论使用 RT-Thread 的 CAN 设备驱动收发数据的思路。

首先是数据的接收，stm32f4 拥有 2 个 CAN 外设，所有电机和使用 CAN 总线的设备都挂载在这两条总线上，但 RT-Thread 的每个 CAN 总线只能通过 rt_device_set_rx_indicate(can_dev, can_rx_call); 注册一个对应的接收回调函数。但不同类型电机，不同 CAN 设备的数据解析处理都是不一样的，我这里的解决思路是：首先创建了一个 usr_callback 文件，用于统一管理 CAN、串口等设备可能用到的用户接收对调函数；将一个大的设备类型回调函数注册到对应 CAN 设备，其中再细分各挂载设备的数据解析，实现如下：

 1#ifdef BSP_USING_CAN
 2rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
 3{
 4    struct rt_can_msg rxmsg = {0};
 5    uint8_t *rxbuff = rxmsg.data;
 6    /* 从 CAN 读取一帧数据 */
 7    rt_device_read(dev, 0, &rxmsg, sizeof(rxmsg));
 8    /* CAN 接收到数据后产生中断，调用此回调函数，然后发送接收信号量 */
 9#ifdef BSP_USING_DJI_MOTOR
10    dji_motot_rx_callback(rxmsg.id, rxbuff);
11#endif /* BSP_USING_DJI_MOTOR */
12    return RT_EOK;
13}
14#endif /* BSP_USING_CAN */

 1/**
 2 * @brief 电机定时器超时回调函数
 3 * @param motor_ptr
 4 */
 5 static void motor_lost_callback(void *motor_ptr)
 6 {
 7     dji_motor_object_t *motor = (dji_motor_object_t *)motor_ptr;
 8//     dji_motor_stop(motor);
 9     LOG_W("[dji_motor] Motor lost, can bus [%s] , id 0x[%x]", motor->can_dev->parent.name, motor->rx_id);
10 }

使用实例

封装完成的电机模块使用示例如下：

 1static struct chassis_controller_t{
 2    pid_object_t *speed_pid;
 3}chassis_controller;
 4static struct gimbal_controller_t{
 5    pid_object_t *speed_pid;
 6    pid_object_t *angle_pid;
 7}gimbal_controlelr;
 8static dji_motor_object_t *chassis_motor;
 9static dji_motor_object_t *gimbal_motor;
10rt_int16_t chassis_control(dji_motor_measure_t measure){
11    static rt_int16_t set = 0;
12    set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
13    return set;
14}
15rt_int16_t gimbal_control(dji_motor_measure_t measure){
16    static rt_int16_t set = 0;
17    set = pid_calculate(gimbal_controlelr.speed_pid, measure.speed_rpm, 0);
18    return set;
19}
20static void example_init()
21{
22    pid_config_t chassis_speed_config = {
23            .Kp = 10, // 4.5
24            .Ki = 0,  // 0
25            .Kd = 0,  // 0
26            .IntegralLimit = 3000,
27            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
28            .MaxOut = 12000,
29    };
30    pid_config_t gimbal_speed_config = {
31            .Kp = 50,  // 50
32            .Ki = 200, // 200
33            .Kd = 0,
34            .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
35            .IntegralLimit = 3000,
36            .MaxOut = 20000,
37    };
38    chassis_controller.speed_pid = pid_register(&chassis_speed_config);
39    gimbal_controlelr.speed_pid = pid_register(&gimbal_speed_config);
40    motor_config_t chassis_motor_config = {
41            .motor_type = M3508,
42            .can_name = CAN_CHASSIS,
43            .rx_id = 0x201,
44            .controller = &chassis_controller,
45    };
46    motor_config_t gimbal_motor_config = {
47            .motor_type = GM6020,
48            .can_name = CAN_GIMBAL,
49            .rx_id = 0x206,
50            .controller = &gimbal_controlelr,
51    };
52    chassis_motor = dji_motor_register(&chassis_motor_config, chassis_control);
53    gimbal_motor = dji_motor_register(&gimbal_motor_config, gimbal_control);
54}

到此就可以方便且灵活的配置和使用电机模块啦

仓库地址放这里了 HNU_RM_SHARK_C ，觉得不错可以点个 Star ！

存在问题及优化方向

1. 目前 rt-thread 下 stm32 can驱动似乎仅支持 FIFO0 ，但 stm32f4 系列 can 具备两个 FIFO，如能同时使能所有 FIFO，应该能有效提高性能和稳定性。

2. 电机的离线回调可以增加相应的声光报警。

3. 后续考虑能不能也优化为，read，write，control 等形式。

————————————————

版权声明：本文为RT-Threadwilliam hill官网 用户「螺丝松掉的人」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：https://club.rt-thread.org/ask/article/0a655bcf83494c87.html

———————End———————

点击阅读原文进入官网