From 72884ef96b647ffa5583c57c1ee7e9f17d984e41 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Mon, 5 Jun 2023 22:43:25 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=86os=E4=BB=BB=E5=8A=A1=E7=9A=84=E5=88=9B?= =?UTF-8?q?=E5=BB=BA=E7=BB=9F=E4=B8=80=E8=BD=AC=E7=A7=BB=E5=88=B0app?= =?UTF-8?q?=E5=B1=82=E6=96=B9=E4=BE=BF=E4=BF=AE=E6=94=B9,=E4=BD=BF?= =?UTF-8?q?=E5=BE=97=E6=9E=B6=E6=9E=84=E6=9B=B4=E5=8A=A0=E6=B8=85=E6=99=B0?= =?UTF-8?q?.=E4=BF=AE=E5=A4=8D=E4=BA=86INS=E6=9C=AA=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E5=AF=BC=E8=87=B4=E7=9A=84=E8=AF=BB=E5=8F=96=E9=94=99?= =?UTF-8?q?=E8=AF=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 6 +- Src/freertos.c | 98 +++-------------------------- Src/main.c | 2 +- application/robot.c | 9 ++- application/robot_def.h | 1 - application/robot_task.h | 111 +++++++++++++++++++++++++++++++++ modules/algorithm/controller.c | 2 +- modules/imu/ins_task.c | 5 ++ modules/imu/ins_task.h | 2 + 9 files changed, 136 insertions(+), 100 deletions(-) create mode 100644 application/robot_task.h diff --git a/README.md b/README.md index d668771..d7d2a13 100644 --- a/README.md +++ b/README.md @@ -3,10 +3,12 @@ > **代码参考了哈工深南宫小樱战队的框架设计和RMUA官方开源RoboRTS-firmware框架，在此鸣谢。** > 每个bsp/module/application都有对应文档，建议阅读之后再看代码&进行开发。框架的搭建思路和讲解视频戳这里：[basic_framework讲解](https://www.bilibili.com/video/BV1Bd4y1E7CN)。 -> 开发之前必看的文档：README.md & VSCode+Ozone使用方法.md + 修改HAL配置时文件目录的更改.md。开发app层请看application目录下的文档，若要开发module以及bsp务必把上层文档也浏览一遍以熟悉接口定义的方式。 +> 开发之前必看的文档：**README.md & VSCode+Ozone使用方法.md** 。开发app层请看application目录下的文档，若要开发module以及bsp务必把上层文档也浏览一遍以熟悉接口定义的方式。 > **程序的运行流程和框架所有app/module/bsp的数据流图直接拉到本文档底部。** -此框架为机器人通用设计，当前的app层是为步兵设计的。不同的机器人只需要重新编写应用层。在我们的战队仓库中有英雄、工程、哨兵等兵种，可作参考。 +此框架为机器人通用设计，当前的app层是为步兵设计的。不同的机器人只需要重新编写应用层。在我们的战队仓库中有英雄、工程、哨兵、平衡步兵等兵种，可作参考。 + +此框架在RoboMaster A型开发板的移植也已在组织仓库中提供。 [TOC] diff --git a/Src/freertos.c b/Src/freertos.c index 63f5c16..c7905f6 100644 --- a/Src/freertos.c +++ b/Src/freertos.c @@ -25,14 +25,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ -#include "ins_task.h" -#include "motor_task.h" -#include "led_task.h" -#include "referee_task.h" -#include "master_process.h" -#include "daemon.h" -#include "robot.h" -#include "HT04.h" + /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -52,26 +45,13 @@ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN Variables */ -osThreadId insTaskHandle; -osThreadId ledTaskHandle; -osThreadId robotTaskHandle; -osThreadId motorTaskHandle; -osThreadId daemonTaskHandle; -osThreadId uiTaskHandle; + /* USER CODE END Variables */ osThreadId defaultTaskHandle; /* Private function prototypes -----------------------------------------------*/ /* USER CODE BEGIN FunctionPrototypes */ -void StartINSTASK(void const *argument); -void StartMOTORTASK(void const *argument); - -void StartDAEMONTASK(void const *argument); - -void StartROBOTTASK(void const *argument); - -void StartUITASK(void const *argument); /* USER CODE END FunctionPrototypes */ void StartDefaultTask(void const *argument); @@ -129,21 +109,6 @@ void MX_FREERTOS_Init(void) /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ - osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024); - insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行 - // 后续修改为读取传感器数据准备好的中断处理, - - osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256); - motorTaskHandle = osThreadCreate(osThread(motortask), NULL); - - osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 128); - daemonTaskHandle = osThreadCreate(osThread(daemontask), NULL); - - osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024); - defaultTaskHandle = osThreadCreate(osThread(robottask), NULL); - - osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512); - defaultTaskHandle = osThreadCreate(osThread(uitask), NULL); /* USER CODE END RTOS_THREADS */ } @@ -158,7 +123,7 @@ void MX_FREERTOS_Init(void) void StartDefaultTask(void const *argument) { /* init code for USB_DEVICE */ - MX_USB_DEVICE_Init(); + MX_USB_DEVICE_Init(); // USB初始化 /* USER CODE BEGIN StartDefaultTask */ vTaskDelete(NULL); // 删除默认任务,防止占用CPU /* USER CODE END StartDefaultTask */ @@ -166,56 +131,9 @@ void StartDefaultTask(void const *argument) /* Private application code --------------------------------------------------*/ /* USER CODE BEGIN Application */ -void StartINSTASK(void const *argument) -{ - while (1) - { - // 1kHz - INS_Task(); - VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织 - osDelay(1); - } -} - -void StartMOTORTASK(void const *argument) -{ - // 若使用HT电机则取消本行注释,该接口会为注册了的电机设备创建线程 - // HTMotorControlInit(); - while (1) - { - // 500Hz - MotorControlTask(); - osDelay(2); - } -} - -void StartDAEMONTASK(void const *argument) -{ - while (1) - { - // 100Hz - DaemonTask(); - osDelay(10); - } -} - -void StartROBOTTASK(void const *argument) -{ - while (1) - { - // 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz - RobotTask(); - osDelay(5); - } -} - -void StartUITASK(void const *argument) -{ - My_UI_init(); - while (1) - { - Referee_Interactive_task(); // 每次给裁判系统发送完一包数据后,挂起一次,防止卡在裁判系统发送中,详见Referee_Interactive_task函数的refereeSend(); - osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换 - } -} +/** + * @brief 注意,为方便统一管理, + * 所有的机器人任务都在applicatoin/robot_task.h中进行 + * + */ /* USER CODE END Application */ diff --git a/Src/main.c b/Src/main.c index a8c16fa..5a1f4b0 100644 --- a/Src/main.c +++ b/Src/main.c @@ -119,7 +119,7 @@ int main(void) MX_CRC_Init(); MX_DAC_Init(); /* USER CODE BEGIN 2 */ - RobotInit(); + RobotInit(); // 唯一的初始化函数 /* USER CODE END 2 */ /* Call init function for freertos objects (in freertos.c) */ diff --git a/application/robot.c b/application/robot.c index 3e4de5d..3e0c5c8 100644 --- a/application/robot.c +++ b/application/robot.c @@ -1,6 +1,7 @@ #include "bsp_init.h" #include "robot.h" #include "robot_def.h" +#include "robot_task.h" // 编译warning,提醒开发者修改机器人参数 #ifndef ROBOT_DEF_PARAM_WARNING @@ -18,10 +19,6 @@ #include "robot_cmd.h" #endif -#ifdef BALANCE_BAORD -#include "balance.h" -#endif // BALANCE_BOARD - void RobotInit() { @@ -42,6 +39,8 @@ void RobotInit() ChassisInit(); #endif + OSTaskInit(); // 创建基础任务 + // 初始化完成,开启中断 __enable_irq(); } @@ -58,4 +57,4 @@ void RobotTask() ChassisTask(); #endif -} +} \ No newline at end of file diff --git a/application/robot_def.h b/application/robot_def.h index d21e764..3457352 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -20,7 +20,6 @@ #define ONE_BOARD // 单板控制整车 // #define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 -// #define BALANCE_BOARD //启用平衡底盘,则默认双板且当前板位底盘,目前不支持!请勿使用! #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 // #define VISION_USE_UART // 使用串口发送视觉数据 diff --git a/application/robot_task.h b/application/robot_task.h new file mode 100644 index 0000000..c7b1aa0 --- /dev/null +++ b/application/robot_task.h @@ -0,0 +1,111 @@ +#pragma once + +#include "FreeRTOS.h" +#include "task.h" +#include "main.h" +#include "cmsis_os.h" + +#include "ins_task.h" +#include "motor_task.h" +#include "referee_task.h" +#include "master_process.h" +#include "daemon.h" +#include "HT04.h" + +osThreadId insTaskHandle; +osThreadId robotTaskHandle; +osThreadId motorTaskHandle; +osThreadId daemonTaskHandle; +osThreadId uiTaskHandle; + +void StartINSTASK(void const *argument); +void StartMOTORTASK(void const *argument); +void StartDAEMONTASK(void const *argument); +void StartROBOTTASK(void const *argument); +void StartUITASK(void const *argument); + +/** + * @brief 初始化机器人任务,所有持续运行的任务都在这里初始化 + * + */ +void OSTaskInit() +{ + osThreadDef(instask, StartINSTASK, osPriorityAboveNormal, 0, 1024); + insTaskHandle = osThreadCreate(osThread(instask), NULL); // 由于是阻塞读取传感器,为姿态解算设置较高优先级,确保以1khz的频率执行 + // 后续修改为读取传感器数据准备好的中断处理, + + osThreadDef(motortask, StartMOTORTASK, osPriorityNormal, 0, 256); + motorTaskHandle = osThreadCreate(osThread(motortask), NULL); + + osThreadDef(daemontask, StartDAEMONTASK, osPriorityNormal, 0, 128); + daemonTaskHandle = osThreadCreate(osThread(daemontask), NULL); + + osThreadDef(robottask, StartROBOTTASK, osPriorityNormal, 0, 1024); + robotTaskHandle = osThreadCreate(osThread(robottask), NULL); + + osThreadDef(uitask, StartUITASK, osPriorityNormal, 0, 512); + uiTaskHandle = osThreadCreate(osThread(uitask), NULL); + + // 若使用HT电机则取消本行注释,该接口会为注册了的电机设备创建线程 + // HTMotorControlInit(); +} + +void StartINSTASK(void const *argument) +{ + static float ins_start, ins_dt; + INS_Init(); // 确保BMI088被正确初始化. + while (1) + { + // 1kHz + ins_start = DWT_GetTimeline_ms(); + INS_Task(); + ins_dt = DWT_GetTimeline_ms() - ins_start; + VisionSend(); // 解算完成后发送视觉数据,但是当前的实现不太优雅,后续若添加硬件触发需要重新考虑结构的组织 + osDelay(1); + } +} + +void StartMOTORTASK(void const *argument) +{ + static float motor_dt, motor_start; + while (1) + { + motor_start = DWT_GetTimeline_ms(); + MotorControlTask(); + motor_dt = DWT_GetTimeline_ms() - motor_start; + osDelay(1); + } +} + +void StartDAEMONTASK(void const *argument) +{ + while (1) + { + // 100Hz + DaemonTask(); + osDelay(10); + } +} + +void StartROBOTTASK(void const *argument) +{ + static float robot_dt, robot_start; + // 200Hz-500Hz,若有额外的控制任务如平衡步兵可能需要提升至1kHz + while (1) + { + robot_start = DWT_GetTimeline_ms(); + RobotTask(); + robot_dt = DWT_GetTimeline_ms() - robot_start; + osDelay(5); + } +} + +void StartUITASK(void const *argument) +{ + My_UI_init(); + while (1) + { + Referee_Interactive_task(); // 每次给裁判系统发送完一包数据后,挂起一次,防止卡在裁判系统发送中,详见Referee_Interactive_task函数的refereeSend(); + osDelay(1); // 即使没有任何UI需要刷新,也挂起一次,防止卡在UITask中无法切换 + } +} diff --git a/modules/algorithm/controller.c b/modules/algorithm/controller.c index 5f0af7b..7d35614 100644 --- a/modules/algorithm/controller.c +++ b/modules/algorithm/controller.c @@ -150,7 +150,7 @@ float PIDCalculate(PIDInstance *pid, float measure, float ref) if (pid->Improve & PID_ErrorHandle) f_PID_ErrorHandle(pid); - pid->dt = DWT_GetDeltaT((void *)&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分 + pid->dt = DWT_GetDeltaT(&pid->DWT_CNT); // 获取两次pid计算的时间间隔,用于积分和微分 // 保存上次的测量值和误差,计算当前error pid->Measure = measure; diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index 4c05f06..f2ca5b2 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -74,6 +74,11 @@ static void InitQuaternion(float *init_q4) attitude_t *INS_Init(void) { + if (!INS.init) + INS.init = 1; + else + return (attitude_t *)&INS.Gyro; + while (BMI088Init(&hspi1, 1) != BMI088_NO_ERROR) ; IMU_Param.scale[X] = 1; diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index 0db399d..4acac19 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -62,6 +62,8 @@ typedef struct float Pitch; float Yaw; float YawTotalAngle; + + uint8_t init; } INS_t; /* 用于修正安装误差的参数 */