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,
上一篇:
Robot Framework: 简明指南 - pybot/robot 命令行选项解析
下一篇:
简易 Twitter 机器人制作教程