基于RT-Thread的RoboMaster電控框架設(shè)計(jì)

由于 RT-Thread 穩(wěn)定高效的內(nèi)核，豐富的文檔教程，積極活躍的社區(qū)氛圍，以及設(shè)備驅(qū)動(dòng)框架、Kconfig、Scons、日志系統(tǒng)、海量的軟件包……很難不選擇 RT-Thread 進(jìn)行項(xiàng)目開發(fā)。但也正是因?yàn)檫@些優(yōu)點(diǎn)的覆蓋面較廣，很多初學(xué)者會(huì)覺得無從下手，但只要步入 RT-Thread 的大門，你就發(fā)現(xiàn)她的美好。這系列文檔將作為本人基于 RT-Thread 開發(fā) RoboMaster 電控框架的記錄與分享，希望能幫助到更多初識(shí) RT-Thread 的小伙伴，也歡迎大家交流分享，指正不足，共同進(jìn)步。

背景
Robomaster 機(jī)器人比賽包含多個(gè)兵種，為了提高研發(fā)效率，模塊化尤為重要，使用 RT-Thread 有助于面對(duì)對(duì)象思想開發(fā)；通過配備的 Kconfig，Scons 等工具可以實(shí)現(xiàn)工程的靈活配置；軟件定時(shí)器可用作各電機(jī)等模塊監(jiān)控，RingBuffer 可以實(shí)現(xiàn)傳感器信息的高效處理 …….

使用的開發(fā)板為大疆的 RoboMaster-C 型開發(fā)板，基礎(chǔ)工程為 rt-thread>bsp>stm32f407-robomaster-c

電機(jī)模塊開發(fā)
使用電機(jī)和電調(diào)均為大疆官方出品，如 2006，3508，6020 等，采用 CAN 通訊方式。

構(gòu)建對(duì)象
首先我們根據(jù)使用的電機(jī)特性，構(gòu)建一個(gè)通用的電機(jī)對(duì)象

/**

@brief DJI intelligent motor typedef
/
typedef struct dji_motor_object
{
rt_device_t can_dev; // 電機(jī)CAN實(shí)例
dji_motor_measure_t measure; // 電機(jī)測(cè)量值
uint32_t tx_id; // 發(fā)送id(主發(fā))
uint32_t rx_id; // 接收id(主收)
/ 分組發(fā)送設(shè)置 /
uint8_t send_group; // 同一幀報(bào)文分組
uint8_t message_num; // 一幀報(bào)文中位置
motor_type_e motor_type; // 電機(jī)類型
motor_working_type_e stop_flag; // 啟停標(biāo)志
/ 監(jiān)控線程相關(guān) /
rt_timer_t timer; // 電機(jī)監(jiān)控定時(shí)器
/ 電機(jī)控制相關(guān) */
void *controller; // 電機(jī)控制器
int16_t (*control)(dji_motor_measure_t measure); // 控制電機(jī)的接口 用戶可以自定義,返回值為16位的電壓或電流值
} dji_motor_object_t;
因?yàn)檫@些電機(jī)我們均使用 CAN 方式進(jìn)行驅(qū)動(dòng)，是 CAN 設(shè)備的延申，于是將 rt_device_t can_dev 父類結(jié)構(gòu)體對(duì)象內(nèi)嵌。

dji_motor_measure_t 結(jié)構(gòu)體中為，電機(jī)控制時(shí)需要用到的一些反饋值，包括電調(diào)直接反饋的數(shù)據(jù)以及進(jìn)一步解算的得出的：

/**

@brief DJI motor feedback
/
typedef struct
{
/ 以下是處理得出的數(shù)據(jù) /
float angle_single_round; // 單圈角度
float speed_aps; // 角速度,單位為:度/秒
float total_angle; // 總角度,注意方向
int32_t total_round; // 總?cè)?shù),注意方向
float target; // 目標(biāo)值(輸出軸扭矩矩/速度/角度(單位度))
/ 以下是電調(diào)直接回傳的數(shù)據(jù) */
uint16_t ecd; // 0-8191
uint16_t last_ecd; // 上一次讀取的編碼器值
int16_t speed_rpm; //電機(jī)的轉(zhuǎn)速值
int16_t real_current; // 實(shí)際轉(zhuǎn)矩電流
uint8_t temperature; // Celsius
} dji_motor_measure_t;
注冊(cè)實(shí)例
通過 dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller) 注冊(cè)對(duì)應(yīng)的電機(jī)實(shí)例，用戶通過 motor_config_t *config 對(duì)實(shí)例進(jìn)行靈活配置：

/**
@brief 電機(jī)初始化,返回一個(gè)電機(jī)實(shí)例

@param config 電機(jī)配置
@return dji_motor_object_t* 電機(jī)實(shí)例指針
*/
dji_motor_object_t *dji_motor_register(motor_config_t *config, void *controller)
{
dji_motor_object_t *object = (dji_motor_object_t )rt_malloc(sizeof(dji_motor_object_t));
rt_memset(object, 0, sizeof(dji_motor_object_t));
// 對(duì)接用戶配置的 motor_config
object->motor_type = config->motor_type; // 6020 or 2006 or 3508
object->rx_id = config->rx_id; // 電機(jī)接收?qǐng)?bào)文的ID
object->control = controller; // 電機(jī)控制器
/ 查找 CAN 設(shè)備 /
object->can_dev = rt_device_find(config->can_name);
// 電機(jī)分組,因?yàn)橹炼?個(gè)電機(jī)可以共用一幀CAN控制報(bào)文
motor_send_grouping(object, config);
// 電機(jī)離線檢測(cè)定時(shí)器相關(guān)
object->timer = rt_timer_create("motor1",
motor_lost_callback,
object, 20,
RT_TIMER_FLAG_PERIODIC);
rt_timer_start(object->timer);
dji_motor_enable(object);
dji_motor_obj[idx++] = object;
return object;
}
/ 電機(jī)配置結(jié)構(gòu)體 */
typedef struct
{
motor_type_e motor_type;
const char *can_name;
uint32_t tx_id; // 發(fā)送id(主發(fā))
uint32_t rx_id; // 接收id(主收)
void *controller; // 電機(jī)控制器
} motor_config_t;
motor_config_t 結(jié)構(gòu)體中的 void *controller 為電機(jī)所使用到的控制器集合，是一個(gè)控制器類型為其成員的結(jié)構(gòu)體變量，如下：

static struct chassis_controller_t{
pid_object_t *speed_pid;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
調(diào)用 dji_motor_object_t *dji_motor_register 時(shí)傳入的 void *controller 為電機(jī)對(duì)應(yīng)的控制器具體實(shí)現(xiàn)，如進(jìn)行 pid 計(jì)算，濾波等，會(huì)賦值給電機(jī)對(duì)象對(duì)應(yīng)的函數(shù)指針，在進(jìn)行電機(jī)控制計(jì)算時(shí)被執(zhí)行，如下：

rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
數(shù)據(jù)處理
電機(jī)對(duì)象離不開對(duì)數(shù)據(jù)穩(wěn)定快速的收發(fā)和解析計(jì)算，接下來展開討論使用 RT-Thread 的 CAN 設(shè)備驅(qū)動(dòng)收發(fā)數(shù)據(jù)的思路。

首先是數(shù)據(jù)的接收，stm32f4 擁有 2 個(gè) CAN 外設(shè)，所有電機(jī)和使用 CAN 總線的設(shè)備都掛載在這兩條總線上，但 RT-Thread 的每個(gè) CAN 總線只能通過 rt_device_set_rx_indicate(can_dev, can_rx_call); 注冊(cè)一個(gè)對(duì)應(yīng)的接收回調(diào)函數(shù)。但不同類型電機(jī)，不同 CAN 設(shè)備的數(shù)據(jù)解析處理都是不一樣的，我這里的解決思路是：首先創(chuàng)建了一個(gè) usr_callback 文件，用于統(tǒng)一管理 CAN、串口等設(shè)備可能用到的用戶接收對(duì)調(diào)函數(shù)；將一個(gè)大的設(shè)備類型回調(diào)函數(shù)注冊(cè)到對(duì)應(yīng) CAN 設(shè)備，其中再細(xì)分各掛載設(shè)備的數(shù)據(jù)解析，實(shí)現(xiàn)如下：

#ifdef BSP_USING_CAN
rt_err_t can_rx_call(rt_device_t dev, rt_size_t size)
{
struct rt_can_msg rxmsg = {0};
uint8_t rxbuff = rxmsg.data;
/ 從 CAN 讀取一幀數(shù)據(jù) /
rt_device_read(dev, 0, &rxmsg, sizeof(rxmsg));
/ CAN 接收到數(shù)據(jù)后產(chǎn)生中斷，調(diào)用此回調(diào)函數(shù)，然后發(fā)送接收信號(hào)量 /
#ifdef BSP_USING_DJI_MOTOR
dji_motot_rx_callback(rxmsg.id, rxbuff);
#endif / BSP_USING_DJI_MOTOR /
return RT_EOK;
}
#endif / BSP_USING_CAN */
但是這其中也有一點(diǎn)問題，rt_err_t can_rx_call(rt_device_t dev, rt_size_t size) 傳入的參數(shù)無法判斷具體的 CAN 設(shè)備來源，因此所有使用到的 CAN 外設(shè)數(shù)據(jù)處理函數(shù)都會(huì)被調(diào)用，但目前問題不大，因?yàn)橥粭l總線上不會(huì)掛載相同 ID 的設(shè)備，這也是一開始就應(yīng)該避免的錯(cuò)誤。

接下來是 CAN 報(bào)文的發(fā)送，調(diào)用 rt_device_write 發(fā)送填充好的 CAN 報(bào)文幀即可。

離線檢測(cè)
這里使用 RT-Thread 的軟件定時(shí)器對(duì)電機(jī)進(jìn)行離線檢測(cè)，當(dāng)超過定時(shí)間沒有接收到對(duì)應(yīng)電機(jī)反饋報(bào)文，則進(jìn)入超時(shí)回調(diào)，并輸出警告日志：

/**

@brief 電機(jī)定時(shí)器超時(shí)回調(diào)函數(shù)
@param motor_ptr
*/
static void motor_lost_callback(void *motor_ptr)
{
dji_motor_object_t *motor = (dji_motor_object_t *)motor_ptr;
// dji_motor_stop(motor);
LOG_W("[dji_motor] Motor lost, can bus [%s] , id 0x[%x]", motor->can_dev->parent.name, motor->rx_id);
}
使用實(shí)例
封裝完成的電機(jī)模塊使用示例如下：

static struct chassis_controller_t{
pid_object_t *speed_pid;
}chassis_controller;
static struct gimbal_controller_t{
pid_object_t *speed_pid;
pid_object_t *angle_pid;
}gimbal_controlelr;
static dji_motor_object_t *chassis_motor;
static dji_motor_object_t *gimbal_motor;
rt_int16_t chassis_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(chassis_controller.speed_pid, measure.speed_rpm, 1000);
return set;
}
rt_int16_t gimbal_control(dji_motor_measure_t measure){
static rt_int16_t set = 0;
set = pid_calculate(gimbal_controlelr.speed_pid, measure.speed_rpm, 0);
return set;
}
static void example_init()
{
pid_config_t chassis_speed_config = {
.Kp = 10, // 4.5
.Ki = 0, // 0
.Kd = 0, // 0
.IntegralLimit = 3000,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.MaxOut = 12000,
};
pid_config_t gimbal_speed_config = {
.Kp = 50, // 50
.Ki = 200, // 200
.Kd = 0,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
.IntegralLimit = 3000,
.MaxOut = 20000,
};
chassis_controller.speed_pid = pid_register(&chassis_speed_config);
gimbal_controlelr.speed_pid = pid_register(&gimbal_speed_config);
motor_config_t chassis_motor_config = {
.motor_type = M3508,
.can_name = CAN_CHASSIS,
.rx_id = 0x201,
.controller = &chassis_controller,
};
motor_config_t gimbal_motor_config = {
.motor_type = GM6020,
.can_name = CAN_GIMBAL,
.rx_id = 0x206,
.controller = &gimbal_controlelr,
};
chassis_motor = dji_motor_register(&chassis_motor_config, chassis_control);
gimbal_motor = dji_motor_register(&gimbal_motor_config, gimbal_control);
}
到此就可以方便且靈活的配置和使用電機(jī)模塊啦。

存在問題及優(yōu)化方向
目前 rt-thread 下 stm32 can驅(qū)動(dòng)似乎僅支持 FIFO0 ，但 stm32f4 系列 can 具備兩個(gè) FIFO，如能同時(shí)使能所有 FIFO，應(yīng)該能有效提高性能和穩(wěn)定性。
電機(jī)的離線回調(diào)可以增加相應(yīng)的聲光報(bào)警。
后續(xù)考慮能不能也優(yōu)化為，read，write，control 等形式。