robofish/rm_balance
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

同步对方从新轮腿到 bc 的改动

Browse Source
This commit is contained in:
Robofish 2026-03-14 16:46:14 +08:00
commit 7f5b8038da
47 changed files with 6403 additions and 515 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

70
.mxproject
View File

File diff suppressed because one or more lines are too long

11
CMakeLists.txt
View File

@ -70,7 +70,6 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/user_math.c
User/component/vmc.c
User/component/ui.c
# User/device sources
User/device/bmi088.c
User/device/buzzer.c
@ -81,17 +80,19 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lk.c
User/device/motor_lz.c
User/device/motor_rm.c
User/device/vision_bridge.c
User/device/vofa.c
User/device/mrobot.c
User/device/referee.c
User/device/supercap.c
# User/module sources
User/module/balance_chassis.c
User/module/config.c
User/module/gimbal.c
User/module/shoot.c
User/module/cmd/cmd.c
User/module/cmd/cmd_adapter.c
User/module/cmd/cmd_behavior.c
User/module/cmd/cmd_example.c
User/module/vision_bridge.c
# User/task sources
User/task/ai.c
@ -107,6 +108,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/cli.c
User/task/user_task.c
User/task/vofa.c
User/task/cmd.c
User/task/ref_main.c
)
# Add include paths

2
Core/Inc/stm32h7xx_it.h
View File

@ -70,6 +70,8 @@ void USART3_IRQHandler(void);
void EXTI15_10_IRQHandler(void);
void DMA1_Stream7_IRQHandler(void);
void UART5_IRQHandler(void);
void DMA2_Stream0_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void UART7_IRQHandler(void);
void ADC3_IRQHandler(void);
void BDMA_Channel0_IRQHandler(void);

13
Core/Src/dma.c
View File

@ -41,6 +41,7 @@ void MX_DMA_Init(void)
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
__HAL_RCC_DMA2_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Stream0_IRQn interrupt configuration */
@ -61,12 +62,12 @@ void MX_DMA_Init(void)
/* DMA1_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
/* DMA1_Stream6_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
/* DMA1_Stream7_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
/* DMA2_Stream0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
/* DMA2_Stream1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
}

28
Core/Src/stm32h7xx_it.c
View File

@ -435,6 +435,34 @@ void UART5_IRQHandler(void)
/* USER CODE END UART5_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream0 global interrupt.
*/
void DMA2_Stream0_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream0_IRQn 0 */
/* USER CODE END DMA2_Stream0_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart10_rx);
/* USER CODE BEGIN DMA2_Stream0_IRQn 1 */
/* USER CODE END DMA2_Stream0_IRQn 1 */
}
/**
* @brief This function handles DMA2 stream1 global interrupt.
*/
void DMA2_Stream1_IRQHandler(void)
{
/* USER CODE BEGIN DMA2_Stream1_IRQn 0 */
/* USER CODE END DMA2_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart10_tx);
/* USER CODE BEGIN DMA2_Stream1_IRQn 1 */
/* USER CODE END DMA2_Stream1_IRQn 1 */
}
/**
* @brief This function handles UART7 global interrupt.
*/

4
Core/Src/usart.c
View File

@ -602,7 +602,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
/* USART10 DMA Init */
/* USART10_RX Init */
hdma_usart10_rx.Instance = DMA1_Stream6;
hdma_usart10_rx.Instance = DMA2_Stream0;
hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX;
hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE;
@ -620,7 +620,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx);
/* USART10_TX Init */
hdma_usart10_tx.Instance = DMA1_Stream7;
hdma_usart10_tx.Instance = DMA2_Stream1;
hdma_usart10_tx.Init.Request = DMA_REQUEST_USART10_TX;
hdma_usart10_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart10_tx.Init.PeriphInc = DMA_PINC_DISABLE;

10
CtrBoard-H7_ALL.ioc
View File

@ -140,7 +140,7 @@ Dma.UART5_RX.3.SyncSignalID=NONE
Dma.USART10_RX.6.Direction=DMA_PERIPH_TO_MEMORY
Dma.USART10_RX.6.EventEnable=DISABLE
Dma.USART10_RX.6.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART10_RX.6.Instance=DMA1_Stream6
Dma.USART10_RX.6.Instance=DMA2_Stream0
Dma.USART10_RX.6.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART10_RX.6.MemInc=DMA_MINC_ENABLE
Dma.USART10_RX.6.Mode=DMA_NORMAL
@ -158,7 +158,7 @@ Dma.USART10_RX.6.SyncSignalID=NONE
Dma.USART10_TX.7.Direction=DMA_MEMORY_TO_PERIPH
Dma.USART10_TX.7.EventEnable=DISABLE
Dma.USART10_TX.7.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART10_TX.7.Instance=DMA1_Stream7
Dma.USART10_TX.7.Instance=DMA2_Stream1
Dma.USART10_TX.7.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART10_TX.7.MemInc=DMA_MINC_ENABLE
Dma.USART10_TX.7.Mode=DMA_NORMAL
@ -254,6 +254,7 @@ GPIO.groupedBy=Group By Peripherals
KeepUserPlacement=false
MMTAppRegionsCount=0
MMTConfigApplied=false
MMTSectionSuffix=_Section
Mcu.CPN=STM32H723VGT6
Mcu.Family=STM32H7
Mcu.IP0=ADC1
@ -372,8 +373,8 @@ NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream1_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@ -613,6 +614,7 @@ ProjectManager.DeletePrevious=true
ProjectManager.DeviceId=STM32H723VGTx
ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.12.1
ProjectManager.FreePins=false
ProjectManager.FreePinsContext=
ProjectManager.HalAssertFull=false
ProjectManager.HeapSize=0x1000
ProjectManager.KeepUserCode=true

301
User/component/ui.c Normal file
View File

@ -0,0 +1,301 @@
/*
UI相关命令
*/
#include "component/ui.h"
#include <stdio.h>
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end) {
if (grapic_line == NULL) return -1;
snprintf((char *)grapic_line->name, 2, "%s", name);
grapic_line->layer = layer;
grapic_line->type_op = type_op;
grapic_line->type_ele = 0;
grapic_line->color = color;
grapic_line->width = width;
grapic_line->x_start = x_start;
grapic_line->y_start = y_start;
grapic_line->x_end = x_end;
grapic_line->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end) {
if (grapic_rectangle == NULL) return -1;
snprintf((char *)grapic_rectangle->name, 2, "%s", name);
grapic_rectangle->type_op = type_op;
grapic_rectangle->type_ele = 1;
grapic_rectangle->layer = layer;
grapic_rectangle->color = color;
grapic_rectangle->width = width;
grapic_rectangle->x_start = x_start;
grapic_rectangle->y_start = y_start;
grapic_rectangle->x_end = x_end;
grapic_rectangle->y_end = y_end;
return 0;
}
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius) {
if (grapic_cycle == NULL) return -1;
snprintf((char *)grapic_cycle->name, 2, "%s", name);
grapic_cycle->type_op = type_op;
grapic_cycle->layer = layer;
grapic_cycle->type_ele = 2;
grapic_cycle->color = color;
grapic_cycle->width = width;
grapic_cycle->x_start = x_center;
grapic_cycle->y_start = y_center;
grapic_cycle->radius = radius;
return 0;
}
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis) {
if (grapic_oval == NULL) return -1;
snprintf((char *)grapic_oval->name, 2, "%s", name);
grapic_oval->type_op = type_op;
grapic_oval->type_ele = 3;
grapic_oval->layer = layer;
grapic_oval->color = color;
grapic_oval->width = width;
grapic_oval->x_start = x_center;
grapic_oval->y_start = y_center;
grapic_oval->x_end = x_semiaxis;
grapic_oval->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
if (grapic_arc == NULL) return -1;
snprintf((char *)grapic_arc->name, 2, "%s", name);
grapic_arc->type_op = type_op;
grapic_arc->type_ele = 4;
grapic_arc->layer = layer;
grapic_arc->color = color;
grapic_arc->angle_start = angle_start;
grapic_arc->angle_end = angle_end;
grapic_arc->width = width;
grapic_arc->x_start = x_center;
grapic_arc->y_start = y_center;
grapic_arc->x_end = x_semiaxis;
grapic_arc->y_end = y_semiaxis;
return 0;
}
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low) {
if (grapic_floating == NULL) return -1;
snprintf((char *)grapic_floating->name, 2, "%s", name);
grapic_floating->type_op = type_op;
grapic_floating->type_ele = 5;
grapic_floating->layer = layer;
grapic_floating->color = color;
grapic_floating->angle_start = font_size;
grapic_floating->angle_end = digits;
grapic_floating->width = width;
grapic_floating->x_start = x_start;
grapic_floating->y_start = y_start;
grapic_floating->radius = float_high;
grapic_floating->x_end = float_middle;
grapic_floating->y_end = float_low;
return 0;
}
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low) {
if (grapic_integer == NULL) return -1;
snprintf((char *)grapic_integer->name, 2, "%s", name);
grapic_integer->type_op = type_op;
grapic_integer->type_ele = 6;
grapic_integer->layer = layer;
grapic_integer->color = color;
grapic_integer->angle_start = font_size;
grapic_integer->width = width;
grapic_integer->x_start = x_start;
grapic_integer->y_start = y_start;
grapic_integer->radius = int32_t_high;
grapic_integer->x_end = int32_t_middle;
grapic_integer->y_end = int32_t_low;
return 0;
}
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character) {
if (grapic_character == NULL) return -1;
snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
grapic_character->grapic.type_op = type_op;
grapic_character->grapic.type_ele = 7;
grapic_character->grapic.layer = layer;
grapic_character->grapic.color = color;
grapic_character->grapic.angle_start = font_size;
grapic_character->grapic.angle_end = length;
grapic_character->grapic.width = width;
grapic_character->grapic.x_start = x_start;
grapic_character->grapic.y_start = y_start;
snprintf((char *)grapic_character->character, 29, "%s", character);
return 0;
}
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
if (del == NULL) return -1;
del->del_operation = opt;
del->layer = layer;
return 0;
}

284
User/component/ui.h Normal file
View File

@ -0,0 +1,284 @@
/*
UI相关命令
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <string.h>
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define UI_DEL_OPERATION_NOTHING (0)
#define UI_DEL_OPERATION_DEL (1)
#define UI_DEL_OPERATION_DEL_ALL (2)
#define UI_GRAPIC_OPERATION_NOTHING (0)
#define UI_GRAPIC_OPERATION_ADD (1)
#define UI_GRAPIC_OPERATION_REWRITE (2)
#define UI_GRAPIC_OPERATION_DEL (3)
#define UI_GRAPIC_LAYER_CONST (0)
#define UI_GRAPIC_LAYER_AUTOAIM (1)
#define UI_GRAPIC_LAYER_CHASSIS (2)
#define UI_GRAPIC_LAYER_CAP (3)
#define UI_GRAPIC_LAYER_GIMBAL (4)
#define UI_GRAPIC_LAYER_SHOOT (5)
#define UI_GRAPIC_LAYER_CMD (6)
#define UI_DEFAULT_WIDTH (0x01)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define UI_CHAR_DEFAULT_WIDTH (0x02)
typedef enum {
RED_BLUE,
YELLOW,
GREEN,
ORANGE,
PURPLISH_RED,
PINK,
CYAN,
BLACK,
WHITE
} UI_Color_t;
typedef struct __packed {
uint8_t op;
uint8_t num_layer;
} UI_InterStudent_UIDel_t;
typedef struct __packed {
uint8_t name[3];
uint8_t type_op : 3;
uint8_t type_ele : 3;
uint8_t layer : 4;
uint8_t color : 4;
uint16_t angle_start : 9;
uint16_t angle_end : 9;
uint16_t width : 10;
uint16_t x_start : 11;
uint16_t y_start : 11;
uint16_t radius : 10;
uint16_t x_end : 11;
uint16_t y_end : 11;
} UI_Ele_t;
typedef struct __packed {
UI_Ele_t grapic;
} UI_Drawgrapic_1_t;
typedef struct __packed {
UI_Ele_t grapic[2];
} UI_Drawgrapic_2_t;
typedef struct __packed {
UI_Ele_t grapic[5];
} UI_Drawgrapic_5_t;
typedef struct __packed {
UI_Ele_t grapic[7];
} UI_Drawgrapic_7_t;
typedef struct __packed {
UI_Ele_t grapic;
uint8_t character[30];
} UI_Drawcharacter_t;
typedef struct __packed {
uint8_t del_operation;
uint8_t layer;
} UI_Del_t;
/**
* @brief UI_绘制直线段
*
* @param grapic_line 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 终点x坐标
* @param y_end 终点y坐标
* @return int8_t
*/
int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t x_end,
uint16_t y_end);
/**
* @brief UI_绘制矩形
*
* @param grapic_rectangle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param x_end 对角顶点x坐标
* @param y_end 对角顶点y坐标
* @return int8_t
*/
int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t width, uint16_t x_start, uint16_t y_start,
uint16_t x_end, uint16_t y_end);
/**
* @brief UI_绘制正圆
*
* @param grapic_cycle 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param radius 半径
* @return int8_t
*/
int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t radius);
/**
* @brief UI_绘制椭圆
*
* @param grapic_oval 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t width,
uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
uint16_t y_semiaxis);
/**
* @brief UI_绘制圆弧
*
* @param grapic_arc 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param angle_start 起始角度
* @param angle_end 终止角度
* @param width 线条宽度
* @param x_center 圆心x坐标
* @param y_center 圆心y坐标
* @param x_semiaxis x半轴长度
* @param y_semiaxis y半轴长度
* @return int8_t
*/
int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
uint8_t layer, uint8_t color, uint16_t angle_start,
uint16_t angle_end, uint16_t width, uint16_t x_center,
uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
/**
* @brief UI_绘制浮点数
*
* @param grapic_float 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param digits 小数点后有效位数
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param float_high 32位浮点数
* @param float_middle 32位浮点数
* @param float_low 32位浮点数
* @return int8_t
*/
int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t digits, uint16_t width,
uint16_t x_start, uint16_t y_start, uint16_t float_high,
uint16_t float_middle, uint16_t float_low);
/**
* @brief UI_绘制整型数
*
* @param grapic_integer 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param int32_t_high 32位整型数
* @param int32_t_middle 32位整型数
* @param int32_t_low 32位整型数
* @return int8_t
*/
int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t width, uint16_t x_start,
uint16_t y_start, uint16_t int32_t_high,
uint16_t int32_t_middle, uint16_t int32_t_low);
/**
* @brief UI_绘制字符
*
* @param grapic_character 结构体
* @param name 图形名首地址
* @param type_op 操作类型
* @param layer 图层数
* @param color 颜色
* @param font_size 字体大小
* @param length 字符长度
* @param width 线条宽度
* @param x_start 起点x坐标
* @param y_start 起点y坐标
* @param character 字符串首地址
* @return int8_t
*/
int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
uint8_t type_op, uint8_t layer, uint8_t color,
uint16_t font_size, uint16_t length, uint16_t width,
uint16_t x_start, uint16_t y_start,
const char *character);
/**
* @brief UI_删除图层
*
* @param del 结构体
* @param opt 操作
* @param layer 图层
* @return int8_t
*/
int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
#ifdef __cplusplus
}
#endif

16
User/component/user_math.c
View File

@ -128,7 +128,21 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
// __NOP();
// }
// }
/* USER FUNCTION BEGIN */
/**
* @brief 按比例缩放
*
* @param a 传入值1
* @param b 传入值2
*/
inline void ScaleSumTo1(float *a, float *b) {
float sum = *a + *b;
if (sum > 1.0f) {
float scale = 1.0f / sum;
*a *= scale;
*b *= scale;
}
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

1
User/component/user_math.h
View File

@ -174,6 +174,7 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
// */
// void VerifyFailed(const char *file, uint32_t line);
void ScaleSumTo1(float *a, float *b);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

3
User/device/device.h
View File

@ -27,6 +27,9 @@ extern "C" {
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3)
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4)
#define SIGNAL_REFEREE_RAW_REDY (1u << 5)
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 6)
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 7)
/* AUTO GENERATED SIGNALS END */
/* USER SIGNALS BEGIN */

843
User/device/referee.c Normal file
View File

@ -0,0 +1,843 @@
/*
裁判系统抽象。
*/
/* Includes ---------------------------------------------------------------- */
#include "device.h"
#include <string.h>
//#include "bsp\delay.h"
#include "bsp\uart.h"
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
#include "device\referee.h"
// #include "module\cmd\cmd.h"
/* Private define ----------------------------------------------------------- */
#define REF_HEADER_SOF (0xA5)
#define REF_LEN_RX_BUFF (0xFF)
#define REF_UI_FAST_REFRESH_FREQ (50)
#define REF_UI_SLOW_REFRESH_FREQ (0.2f)
#define REF_UI_BOX_UP_OFFSET (4)
#define REF_UI_BOX_BOT_OFFSET (-14)
#define REF_UI_RIGHT_START_POS (0.85f)
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
// static uint8_t rxbuf[REF_LEN_RX_BUFF];
uint8_t rxbuf[REF_LEN_RX_BUFF];
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void Referee_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void Referee_IdleLineCallback(void) {
HAL_UART_AbortReceive_IT(BSP_UART_GetHandle(BSP_UART_REF));
}
static void Referee_AbortRxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_RAW_REDY);
}
static void RefereeFastRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_FAST_REFRESH_UI);
}
static void RefereeSlowRefreshTimerCallback(void *arg) {
(void)arg;
osThreadFlagsSet(thread_alert, SIGNAL_REFEREE_SLOW_REFRESH_UI);
}
/* Exported functions ------------------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const Referee_Screen_t *screen) {
if (ref == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
ui->screen = screen;
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_RX_CPLT_CB,
Referee_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_ABORT_RX_CPLT_CB,
Referee_AbortRxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_REF, BSP_UART_IDLE_LINE_CB,
Referee_IdleLineCallback);
uint32_t fast_period_ms = (uint32_t)(1000.0f / REF_UI_FAST_REFRESH_FREQ);
uint32_t slow_period_ms = (uint32_t)(1000.0f / REF_UI_SLOW_REFRESH_FREQ);
ref->ui_fast_timer_id =
osTimerNew(RefereeFastRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
ref->ui_slow_timer_id =
osTimerNew(RefereeSlowRefreshTimerCallback, osTimerPeriodic, NULL, NULL);
osTimerStart(ref->ui_fast_timer_id, fast_period_ms);
osTimerStart(ref->ui_slow_timer_id, slow_period_ms);
// __HAL_UART_ENABLE_IT(BSP_UART_GetHandle(BSP_UART_REF), UART_IT_IDLE);
inited = true;
return 0;
}
int8_t Referee_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_REF));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_REF));
return 0;
}
int8_t Referee_StartReceiving(Referee_t *ref) {
(void)ref;
if ( BSP_UART_Receive(BSP_UART_REF, rxbuf, REF_LEN_RX_BUFF,0)
== BSP_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool Referee_CheckTXReady() {
return BSP_UART_GetHandle(BSP_UART_REF)->gState == HAL_UART_STATE_READY;
}
void Referee_HandleOffline(Referee_t *referee) {
referee->ref_status = REF_STATUS_OFFLINE;
}
int8_t Referee_Parse(Referee_t *ref) {
REF_SWITCH_STATUS(*ref, REF_STATUS_RUNNING);
uint32_t data_length =
REF_LEN_RX_BUFF -
__HAL_DMA_GET_COUNTER(BSP_UART_GetHandle(BSP_UART_REF)->hdmarx);
uint8_t index = 0;
uint8_t packet_shift;
uint8_t packet_length;
while (index < data_length && rxbuf[index] == REF_HEADER_SOF) {
packet_shift = index;
Referee_Header_t *header = (Referee_Header_t *)(rxbuf + index);
index += sizeof(*header);
if (index - packet_shift >= data_length) goto error;
if (!CRC8_Verify((uint8_t *)header, sizeof(*header))) goto error;
if (header->sof != REF_HEADER_SOF) goto error;
Referee_CMDID_t *cmd_id = (Referee_CMDID_t *)(rxbuf + index);
index += sizeof(*cmd_id);
if (index - packet_shift >= data_length) goto error;
void *target = (rxbuf + index);
void *origin;
size_t size;
switch (*cmd_id) {
case REF_CMD_ID_GAME_STATUS:
origin = &(ref->game_status);
size = sizeof(ref->game_status);
break;
case REF_CMD_ID_GAME_RESULT:
origin = &(ref->game_result);
size = sizeof(ref->game_result);
break;
case REF_CMD_ID_GAME_ROBOT_HP:
origin = &(ref->game_robot_hp);
size = sizeof(ref->game_robot_hp);
break;
case REF_CMD_ID_WARNING:
origin = &(ref->warning);
size = sizeof(ref->warning);
break;
case REF_CMD_ID_DART_COUNTDOWN:
origin = &(ref->dart_countdown);
size = sizeof(ref->dart_countdown);
break;
case REF_CMD_ID_ROBOT_STATUS:
origin = &(ref->robot_status);
size = sizeof(ref->robot_status);
break;
case REF_CMD_ID_POWER_HEAT_DATA:
origin = &(ref->power_heat);
size = sizeof(ref->power_heat);
break;
case REF_CMD_ID_ROBOT_POS:
origin = &(ref->robot_pos);
size = sizeof(ref->robot_pos);
break;
case REF_CMD_ID_ROBOT_BUFF:
origin = &(ref->robot_buff);
size = sizeof(ref->robot_buff);
break;
case REF_CMD_ID_ROBOT_DMG:
origin = &(ref->robot_danage);
size = sizeof(ref->robot_danage);
break;
case REF_CMD_ID_SHOOT_DATA:
origin = &(ref->shoot_data);
size = sizeof(ref->shoot_data);
break;
case REF_CMD_ID_BULLET_REMAINING:
origin = &(ref->bullet_remain);
size = sizeof(ref->bullet_remain);
break;
case REF_CMD_ID_RFID:
origin = &(ref->rfid);
size = sizeof(ref->rfid);
break;
case REF_CMD_ID_DART_CLIENT:
origin = &(ref->dart_client);
size = sizeof(ref->dart_client);
break;
case REF_CMD_ID_GROUND_ROBOT_POS:
origin = &(ref->ground_robot_pos);
size = sizeof(ref->ground_robot_pos);
break;
case REF_CMD_ID_RADAR_MASK_PROC:
origin = &(ref->radar_mark);
size = sizeof(ref->radar_mark);
break;
case REF_CMD_ID_SENTRY_AUTO_DEC:
origin = &(ref->sentry_info);
size = sizeof(ref->sentry_info);
break;
case REF_CMD_ID_RADAR_AUTO_DEC:
origin = &(ref->radar_info);
size = sizeof(ref->radar_info);
break;
case REF_CMD_ID_INTER_STUDENT:
origin = &(ref->robot_interaction);
size = sizeof(ref->robot_interaction);
break;
case REF_CMD_ID_INTER_STUDENT_CUSTOM:
origin = &(ref->custom_robot);
size = sizeof(ref->custom_robot);
break;
case REF_CMD_ID_CLIENT_MAP:
origin = &(ref->map_command);
size = sizeof(ref->map_command);
break;
case REF_CMD_ID_KEYBOARD_MOUSE:
origin = &(ref->keyboard_mouse_t);
size = sizeof(ref->keyboard_mouse_t);
break;
case REF_CMD_ID_CLIENT_MAP_REC_RADAR:
origin = &(ref->map_robot_data);
size = sizeof(ref->map_robot_data);
break;
case REF_CMD_ID_CUSTOMER_CONTROLLER_CLIENT_INTERACT:
origin = &(ref->custom_client);
size = sizeof(ref->custom_client);
break;
case REF_CMD_ID_CLIENT_MAP_REC_ROUTE:
origin = &(ref->map_data);
size = sizeof(ref->map_data);
break;
case REF_CMD_ID_CLIENT_MAP_REC_ROBOT:
origin = &(ref->custom_info);
size = sizeof(ref->custom_info);
break;
case REF_CMD_ID_CUSTOMER_CONTROLLER_CLIENT_REC_ROBOT:
origin = &(ref->robot_custom);
size = sizeof(ref->robot_custom);
break;
case REF_CMD_ID_ROBOT_TO_CUSTOMER_CONTROLLER_CLIENT_REC:
origin = &(ref->robot_custom2);
size = sizeof(ref->robot_custom2);
break;
case REF_CMD_ID_OPPSITE_ROBOT_POS:/*0xA01*/
origin = &(ref->robot_custom2);
size = sizeof(ref->robot_custom2);
break;
case REF_CMD_ID_OPPSITE_ROBOT_HP:/*0xA02*/
origin = &(ref->oppsite_robotHP);
size = sizeof(ref->oppsite_robotHP);
break;
case REF_CMD_ID_OPPSITE_ROBOT_BULLET_REMAINING:/*0xA03*/
origin = &(ref->oppsite_bullet_remain);
size = sizeof(ref->oppsite_bullet_remain);
break;
case REF_CMD_ID_OPPSITE_ROBOT_STATUs:/*0xA04*/
origin = &(ref->oppsite_robot_satate);
size = sizeof(ref->oppsite_robot_satate);
break;
case REF_CMD_ID_OPPSITE_ROBOT_BUFF:/*0xA05*/
origin = &(ref->oppsite_robot_buff);
size = sizeof(ref->oppsite_robot_buff);
break;
case REF_CMD_ID_OPPSITE_ROBOT_INTERF_WAVE_SRCRET_KEY:/*0xA06*/
origin = &(ref->opsite_DisturbingWave_key);
size = sizeof(ref->opsite_DisturbingWave_key);
break;
// case REF_CMD_ID_CLIENT_MAP:
// origin = &(ref->client_map);
// size = sizeof(ref->client_map);
default:
return DEVICE_ERR;
}
packet_length = sizeof(Referee_Header_t) + sizeof(Referee_CMDID_t) + size +
sizeof(Referee_Tail_t);
index += size;
if (index - packet_shift >= data_length) goto error;
index += sizeof(Referee_Tail_t);
if (index - packet_shift != packet_length) goto error;
if (CRC16_Verify((uint8_t *)(rxbuf + packet_shift), packet_length))
memcpy(origin, target, size);
else
goto error;
}
return DEVICE_OK;
error:
drop_message++;
return DEVICE_ERR;
}
int8_t Referee_StartSend(uint8_t *data, uint32_t len) {
if (BSP_UART_Transmit(BSP_UART_REF,data, (size_t)len, true) == BSP_OK) {
return DEVICE_OK;
} else
return DEVICE_ERR;
}
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len) {
if (len <= 0 || data == NULL || tmp == NULL) return DEVICE_ERR;
memcpy(tmp, (const void *)data, (size_t)len);
memset(data, 0, (size_t)len);
return DEVICE_OK;
}
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id) {
header->data_cmd_id = cmd_id;
if (sender_id <= REF_BOT_RED_RADER) switch (sender_id) {
case REF_BOT_RED_HERO:
header->sender_ID = REF_BOT_RED_HERO;
header->receiver_ID = REF_CL_RED_HERO;
break;
case REF_BOT_RED_ENGINEER:
header->sender_ID = REF_BOT_RED_ENGINEER;
header->receiver_ID = REF_CL_RED_ENGINEER;
break;
case REF_BOT_RED_INFANTRY_1:
header->sender_ID = REF_BOT_RED_INFANTRY_1;
header->receiver_ID = REF_CL_RED_INFANTRY_1;
break;
case REF_BOT_RED_INFANTRY_2:
header->sender_ID = REF_BOT_RED_INFANTRY_2;
header->receiver_ID = REF_CL_RED_INFANTRY_2;
break;
case REF_BOT_RED_INFANTRY_3:
header->sender_ID = REF_BOT_RED_INFANTRY_3;
header->receiver_ID = REF_CL_RED_INFANTRY_3;
break;
case REF_BOT_RED_DRONE:
header->sender_ID = REF_BOT_RED_DRONE;
header->receiver_ID = REF_CL_RED_DRONE;
break;
case REF_BOT_RED_SENTRY:
header->sender_ID = REF_BOT_RED_SENTRY;
break;
case REF_BOT_RED_RADER:
header->sender_ID = REF_BOT_RED_RADER;
break;
default:
return -1;
}
else
switch (sender_id) {
case REF_BOT_BLU_HERO:
header->sender_ID = REF_BOT_BLU_HERO;
header->receiver_ID = REF_CL_BLU_HERO;
break;
case REF_BOT_BLU_ENGINEER:
header->sender_ID = REF_BOT_BLU_ENGINEER;
header->receiver_ID = REF_CL_BLU_ENGINEER;
break;
case REF_BOT_BLU_INFANTRY_1:
header->sender_ID = REF_BOT_BLU_INFANTRY_1;
header->receiver_ID = REF_CL_BLU_INFANTRY_1;
break;
case REF_BOT_BLU_INFANTRY_2:
header->sender_ID = REF_BOT_BLU_INFANTRY_2;
header->receiver_ID = REF_CL_BLU_INFANTRY_2;
break;
case REF_BOT_BLU_INFANTRY_3:
header->sender_ID = REF_BOT_BLU_INFANTRY_3;
header->receiver_ID = REF_CL_BLU_INFANTRY_3;
break;
case REF_BOT_BLU_DRONE:
header->sender_ID = REF_BOT_BLU_DRONE;
header->receiver_ID = REF_CL_BLU_DRONE;
break;
case REF_BOT_BLU_SENTRY:
header->sender_ID = REF_BOT_BLU_SENTRY;
break;
case REF_BOT_BLU_RADER:
header->sender_ID = REF_BOT_BLU_RADER;
break;
default:
return -1;
}
return 0;
}
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref) {
if (!Referee_CheckTXReady()) return 0;
if (ui->character_counter == 0 && ui->grapic_counter == 0 &&
ui->del_counter == 0)
return 0;
static uint8_t send_data[sizeof(Referee_UI_Drawgrapic_7_t)] = {0};
uint16_t size;
if (ui->del_counter != 0) {
if (ui->del_counter < 0 || ui->del_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Del_t *address = (Referee_UI_Del_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Del_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_DEL,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->del[--ui->del_counter]), &(address->data),
sizeof(UI_Del_t));
address->crc16 =
CRC16_Calc((const uint8_t *)address,
sizeof(Referee_UI_Del_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Del_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
} else if (ui->grapic_counter != 0) {
switch (ui->grapic_counter) {
case 1:
size = sizeof(Referee_UI_Drawgrapic_1_t);
Referee_UI_Drawgrapic_1_t *address_1 =
(Referee_UI_Drawgrapic_1_t *)send_data;
address_1->header.sof = REF_HEADER_SOF;
address_1->header.data_length =
sizeof(UI_Drawgrapic_1_t) + sizeof(Referee_Interactive_Header_t);
address_1->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_1->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_1->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_1->IA_header), REF_STDNT_CMD_ID_UI_DRAW1,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_1->data.grapic),
sizeof(UI_Drawgrapic_1_t));
address_1->crc16 = CRC16_Calc(
(const uint8_t *)address_1,
sizeof(Referee_UI_Drawgrapic_1_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 2:
size = sizeof(Referee_UI_Drawgrapic_2_t);
Referee_UI_Drawgrapic_2_t *address_2 =
(Referee_UI_Drawgrapic_2_t *)send_data;
address_2->header.sof = REF_HEADER_SOF;
address_2->header.data_length =
sizeof(UI_Drawgrapic_2_t) + sizeof(Referee_Interactive_Header_t);
address_2->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_2->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_2->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_2->IA_header), REF_STDNT_CMD_ID_UI_DRAW2,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_2->data.grapic),
sizeof(UI_Drawgrapic_2_t));
address_2->crc16 = CRC16_Calc(
(const uint8_t *)address_2,
sizeof(Referee_UI_Drawgrapic_2_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 3:
case 4:
case 5:
size = sizeof(Referee_UI_Drawgrapic_5_t);
Referee_UI_Drawgrapic_5_t *address_5 =
(Referee_UI_Drawgrapic_5_t *)send_data;
address_5->header.sof = REF_HEADER_SOF;
address_5->header.data_length =
sizeof(UI_Drawgrapic_5_t) + sizeof(Referee_Interactive_Header_t);
address_5->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_5->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_5->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_5->IA_header), REF_STDNT_CMD_ID_UI_DRAW5,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_5->data.grapic),
sizeof(UI_Drawgrapic_5_t));
address_5->crc16 = CRC16_Calc(
(const uint8_t *)address_5,
sizeof(Referee_UI_Drawgrapic_5_t) - sizeof(uint16_t), CRC16_INIT);
break;
case 6:
case 7:
size = sizeof(Referee_UI_Drawgrapic_7_t);
Referee_UI_Drawgrapic_7_t *address_7 =
(Referee_UI_Drawgrapic_7_t *)send_data;
address_7->header.sof = REF_HEADER_SOF;
address_7->header.data_length =
sizeof(UI_Drawgrapic_7_t) + sizeof(Referee_Interactive_Header_t);
address_7->header.crc8 =
CRC8_Calc((const uint8_t *)&(address_7->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address_7->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address_7->IA_header), REF_STDNT_CMD_ID_UI_DRAW7,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->grapic), &(address_7->data.grapic),
sizeof(UI_Drawgrapic_7_t));
address_7->crc16 = CRC16_Calc(
(const uint8_t *)address_7,
sizeof(Referee_UI_Drawgrapic_7_t) - sizeof(uint16_t), CRC16_INIT);
break;
default:
return DEVICE_ERR;
}
if (Referee_StartSend(send_data, size) == HAL_OK) {
ui->grapic_counter = 0;
return DEVICE_OK;
}
} else if (ui->character_counter != 0) {
if (ui->character_counter < 0 ||
ui->character_counter > REF_UI_MAX_STRING_NUM)
return DEVICE_ERR;
Referee_UI_Drawcharacter_t *address =
(Referee_UI_Drawcharacter_t *)send_data;
address->header.sof = REF_HEADER_SOF;
address->header.data_length =
sizeof(UI_Drawcharacter_t) + sizeof(Referee_Interactive_Header_t);
address->header.crc8 =
CRC8_Calc((const uint8_t *)&(address->header),
sizeof(Referee_Header_t) - sizeof(uint8_t), CRC8_INIT);
address->cmd_id = REF_CMD_ID_INTER_STUDENT;
Referee_SetHeader(&(address->IA_header), REF_STDNT_CMD_ID_UI_STR,
ref->robot_status.robot_id);
Referee_MoveData(&(ui->character_data[--ui->character_counter]),
&(address->data.grapic), sizeof(UI_Drawcharacter_t));
address->crc16 = CRC16_Calc(
(const uint8_t *)address,
sizeof(Referee_UI_Drawcharacter_t) - sizeof(uint16_t), CRC16_INIT);
size = sizeof(Referee_UI_Drawcharacter_t);
Referee_StartSend(send_data, size);
return DEVICE_OK;
}
return DEVICE_ERR_NULL;
}
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui) {
if (ref_ui->grapic_counter >= REF_UI_MAX_GRAPIC_NUM ||
ref_ui->grapic_counter < 0)
return NULL;
else
return &(ref_ui->grapic[ref_ui->grapic_counter++]);
}
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui) {
if (ref_ui->character_counter >= REF_UI_MAX_STRING_NUM ||
ref_ui->character_counter < 0)
return NULL;
else
return &(ref_ui->character_data[ref_ui->character_counter++]);
}
UI_Del_t *Referee_GetDelAdd(Referee_UI_t *ref_ui) {
if (ref_ui->del_counter >= REF_UI_MAX_DEL_NUM || ref_ui->del_counter < 0)
return NULL;
else
return &(ref_ui->del[ref_ui->del_counter++]);
}
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, Referee_UI_CMD_t cmd) {
switch (cmd) {
/* Demo */
case UI_NOTHING:
/* 字符 */
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "0",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH, 100, 100, 200, 200, "Demo");
/* 直线 */
UI_DrawLine(Referee_GetGrapicAdd(ref_ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 960, 540,
960, 240);
/* 圆形 */
UI_DrawCycle(Referee_GetGrapicAdd(ref_ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_AUTOAIM, RED_BLUE, UI_DEFAULT_WIDTH, 900,
500, 10);
/* 删除 */
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
break;
case UI_AUTO_AIM_START:
UI_DrawCharacter(Referee_GetCharacterAdd(ref_ui), "1",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_AUTOAIM,
RED_BLUE, UI_DEFAULT_WIDTH * 10, 50, UI_DEFAULT_WIDTH,
ref_ui->screen->width * 0.8,
ref_ui->screen->height * 0.5, "AUTO");
break;
case UI_AUTO_AIM_STOP:
UI_DelLayer(Referee_GetDelAdd(ref_ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_AUTOAIM);
default:
return -1;
}
return 0;
}
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref) {
cap->chassis_power_limit = ref->robot_status.chassis_power_limit;
cap->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
cap->chassis_watt = ref->power_heat.chassis_watt;
cap->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackAI(Referee_ForAI_t *ai, const Referee_t *ref) {
ai->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref) {
chassis->chassis_power_limit = ref->robot_status.chassis_power_limit;
chassis->chassis_pwr_buff = ref->power_heat.chassis_pwr_buff;
chassis->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_PackShoot(Referee_ForShoot_t *shoot, Referee_t *ref) {
memcpy(&(shoot->power_heat), &(ref->power_heat), sizeof(shoot->power_heat));
memcpy(&(shoot->robot_status), &(ref->robot_status),
sizeof(shoot->robot_status));
memcpy(&(shoot->shoot_data), &(ref->shoot_data), sizeof(shoot->shoot_data));
shoot->ref_status = ref->ref_status;
return 0;
}
uint8_t Referee_UIRefresh(Referee_UI_t *ui) {
static uint8_t fsm = 0;
if (osThreadFlagsGet() & SIGNAL_REFEREE_FAST_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_FAST_REFRESH_UI);
switch (fsm) {
case 0: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CHASSIS);
UI_DrawLine(Referee_GetGrapicAdd(ui), "6", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH * 12,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4f + sinf(ui->chassis_ui.angle) * 46.0f,
ui->screen->height * 0.2f + cosf(ui->chassis_ui.angle) * 46.0f);
float start_pos_h = 0.0f;
switch (ui->chassis_ui.mode) {
case CHASSIS_MODE_RELAX:
start_pos_h = 0.68f;
break;
case CHASSIS_MODE_WHELL_LEG_BALANCE:
start_pos_h = 0.66f;
break;
case CHASSIS_MODE_BALANCE_ROTOR:
start_pos_h = 0.64f;
break;
default:
break;
}
if (start_pos_h != 0.0f)
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "8", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CHASSIS, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS - 6,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 44,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 1:
fsm++; UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CAP);
switch (ui->cap_ui.status) {
case SUPERCAP_STATUS_OFFLINE:
UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CAP, YELLOW, 0, 360,
UI_DEFAULT_WIDTH * 5, ui->screen->width * 0.6,
ui->screen->height * 0.2, 50, 50);
break;
break;
case SUPERCAP_STATUS_RUNNING:
UI_DrawArc(Referee_GetGrapicAdd(ui), "9", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CAP, GREEN, 0,
ui->cap_ui.percentage * 360, UI_DEFAULT_WIDTH * 5,
ui->screen->width * 0.6, ui->screen->height * 0.2, 50,
50);
break;
}
break;
case 2: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_GIMBAL);
float start_pos_h = 0.0f;
switch (ui->gimbal_ui.mode) {
case GIMBAL_MODE_RELAX:
start_pos_h = 0.68f;
break;
case GIMBAL_MODE_RELATIVE:
start_pos_h = 0.66f;
break;
case GIMBAL_MODE_ABSOLUTE:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "a", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_GIMBAL, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 54,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 102,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 3: {
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_SHOOT);
float start_pos_h = 0.0f;
switch (ui->shoot_ui.fire) {
case SHOOT_STATE_IDLE:
start_pos_h = 0.68f;
break;
case SHOOT_STATE_READY:
start_pos_h = 0.66f;
break;
case SHOOT_STATE_FIRE:
start_pos_h = 0.64f;
break;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "b", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 114,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 162,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
switch (ui->shoot_ui.mode) {
case SHOOT_MODE_SAFE:
start_pos_h = 0.68f;
break;
case SHOOT_MODE_SINGLE:
start_pos_h = 0.66f;
break;
case SHOOT_MODE_BURST:
start_pos_h = 0.64f;
default:
break;
}
UI_DrawRectangle(
Referee_GetGrapicAdd(ui), "f", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_SHOOT, GREEN, UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 174,
ui->screen->height * start_pos_h + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 222,
ui->screen->height * start_pos_h + REF_UI_BOX_BOT_OFFSET);
break;
}
case 4:
fsm++;
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CMD);
if (ui->cmd_pc) {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 96,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 120,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
} else {
UI_DrawRectangle(Referee_GetGrapicAdd(ui), "c",
UI_GRAPIC_OPERATION_ADD, UI_GRAPIC_LAYER_CMD, GREEN,
UI_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS + 56,
ui->screen->height * 0.4 + REF_UI_BOX_UP_OFFSET,
ui->screen->width * REF_UI_RIGHT_START_POS + 80,
ui->screen->height * 0.4 + REF_UI_BOX_BOT_OFFSET);
}
break;
default:
fsm = 0;
if (ui->del_counter >= REF_UI_MAX_DEL_NUM ||
ui->character_counter > REF_UI_MAX_STRING_NUM ||
ui->grapic_counter > REF_UI_MAX_GRAPIC_NUM)
BSP_UART_GetHandle(BSP_UART_REF)->gState = HAL_UART_STATE_READY;
}
}
if (osThreadFlagsGet() & SIGNAL_REFEREE_SLOW_REFRESH_UI) {
osThreadFlagsClear(SIGNAL_REFEREE_SLOW_REFRESH_UI);
UI_DelLayer(Referee_GetDelAdd(ui), UI_DEL_OPERATION_DEL,
UI_GRAPIC_LAYER_CONST);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "1", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.7, "CHAS GMBL SHOT FIRE");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "2", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.68, "FLLW RELX RELX SNGL");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "3", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.66, "FL35 ABSL SAFE BRST");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "4", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.64, "ROTR RLTV LOAD CONT");
UI_DrawLine(Referee_GetGrapicAdd(ui), "5", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 3,
ui->screen->width * 0.4, ui->screen->height * 0.2,
ui->screen->width * 0.4, ui->screen->height * 0.2 + 50);
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "d", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 10, 80,
UI_CHAR_DEFAULT_WIDTH,
ui->screen->width * REF_UI_RIGHT_START_POS,
ui->screen->height * 0.4, "CTRL RC PC");
UI_DrawCharacter(Referee_GetCharacterAdd(ui), "e", UI_GRAPIC_OPERATION_ADD,
UI_GRAPIC_LAYER_CONST, GREEN, UI_DEFAULT_WIDTH * 20, 80,
UI_CHAR_DEFAULT_WIDTH * 2, ui->screen->width * 0.6 - 26,
ui->screen->height * 0.2 + 10, "CAP");
}
return 0;
}

676
User/device/referee.h Normal file
View File

@ -0,0 +1,676 @@
/*
裁判系统抽象。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include "component\ui.h"
#include "component\user_math.h"
#include "device\device.h"
#include "device\referee_proto_types.h"
#include "device\supercap.h"
#include "module\balance_chassis.h"
#include "module\gimbal.h"
#include "module\shoot.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
#define CHASSIS_POWER_MAX_WITHOUT_REF 50.0f /* 裁判系统离线底盘最大功率 */
#define REF_UI_MAX_GRAPIC_NUM (7)
#define REF_UI_MAX_STRING_NUM (7)
#define REF_UI_MAX_DEL_NUM (3)
#define REF_USER_DATA_MAX_LEN 112
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t sof;
uint16_t data_length;
uint8_t seq;
uint8_t crc8;
} Referee_Header_t;
/* 分辨率配置 */
/* Referee_Status_t, PowerHeat_t, RobotStatus_t, ShootData_t,
ForCap/AI/Chassis/Shoot_t, Screen_t, UI_CMD_t
均定义在 device/referee_proto_types.h（已在上方 include）*/
typedef enum {
REF_CMD_ID_GAME_STATUS = 0x0001,
REF_CMD_ID_GAME_RESULT = 0x0002,
REF_CMD_ID_GAME_ROBOT_HP = 0x0003,
// REF_CMD_ID_DART_STATUS = 0x0004,
REF_CMD_ID_FIELD_EVENTS = 0x0101,
REF_CMD_ID_WARNING = 0x0104,
REF_CMD_ID_DART_COUNTDOWN = 0x0105,
REF_CMD_ID_ROBOT_STATUS = 0x0201,
REF_CMD_ID_POWER_HEAT_DATA = 0x0202,
REF_CMD_ID_ROBOT_POS = 0x0203,
REF_CMD_ID_ROBOT_BUFF = 0x0204,
REF_CMD_ID_ROBOT_DMG = 0x0206,
REF_CMD_ID_SHOOT_DATA = 0x0207,
REF_CMD_ID_BULLET_REMAINING = 0x0208,
REF_CMD_ID_RFID = 0x0209,
REF_CMD_ID_DART_CLIENT = 0x020A,
REF_CMD_ID_GROUND_ROBOT_POS = 0x020B,
REF_CMD_ID_RADAR_MASK_PROC = 0x020C,
REF_CMD_ID_SENTRY_AUTO_DEC = 0x020D,
REF_CMD_ID_RADAR_AUTO_DEC = 0x020E,
REF_CMD_ID_INTER_STUDENT = 0x0301,
REF_CMD_ID_INTER_STUDENT_CUSTOM = 0x0302,
REF_CMD_ID_CLIENT_MAP = 0x0303,
REF_CMD_ID_KEYBOARD_MOUSE = 0x0304,
REF_CMD_ID_CLIENT_MAP_REC_RADAR = 0x0305,
REF_CMD_ID_CUSTOMER_CONTROLLER_CLIENT_INTERACT = 0x0306,
REF_CMD_ID_CLIENT_MAP_REC_ROUTE = 0x0307,
REF_CMD_ID_CLIENT_MAP_REC_ROBOT = 0x0308,
REF_CMD_ID_CUSTOMER_CONTROLLER_CLIENT_REC_ROBOT = 0x0309,
REF_CMD_ID_ROBOT_TO_CUSTOMER_CONTROLLER_CLIENT_REC = 0x0310,
REF_CMD_ID_SET_PICTURE_CHANNEL = 0x0F01,
REF_CMD_ID_QUERY_PICTURE_CHANNEL = 0x0F02,
REF_CMD_ID_OPPSITE_ROBOT_POS = 0x0A01,
REF_CMD_ID_OPPSITE_ROBOT_HP = 0x0A02,
REF_CMD_ID_OPPSITE_ROBOT_BULLET_REMAINING = 0x0A03,
REF_CMD_ID_OPPSITE_ROBOT_STATUs = 0x0A04,
REF_CMD_ID_OPPSITE_ROBOT_BUFF= 0x0A05,
REF_CMD_ID_OPPSITE_ROBOT_INTERF_WAVE_SRCRET_KEY= 0x0A06,
} Referee_CMDID_t;
typedef struct __packed {
uint8_t game_type : 4;
uint8_t game_progress : 4;
uint16_t stage_remain_time;
uint64_t sync_time_stamp;
} Referee_GameStatus_t;
typedef struct __packed {
uint8_t winner;
} Referee_GameResult_t;
typedef struct __packed {
uint16_t ally_1_robot_HP;
uint16_t ally_2_robot_HP;
uint16_t ally_3_robot_HP;
uint16_t ally_4_robot_HP;
uint16_t reserved;
uint16_t ally_7_robot_HP;
uint16_t ally_outpost_HP;
uint16_t ally_base_HP;
} Referee_GameRobotHP_t;
typedef struct __packed {
uint32_t event_data;
} Referee_Event_t;
typedef struct __packed {
uint8_t level;
uint8_t offending_robot_id;
uint8_t count;
} Referee_Warning_t;
typedef struct __packed {
uint8_t dart_remaining_time;
uint16_t dart_info;
} Referee_DartInfo_t;
typedef struct __packed {
float x;
float y;
float angle;
} Referee_RobotPos_t;
typedef struct {
uint8_t recovery_buff;
uint16_t cooling_buff;
uint8_t defence_buff;
uint8_t vulnerability_buff;
uint16_t attack_buff;
uint8_t remaining_energy;
} Referee_RobotBuff_t;
typedef struct __packed {
uint8_t armor_id : 4;
uint8_t damage_type : 4;
} Referee_RobotDamage_t;
typedef struct __packed {
uint16_t bullet_17_remain;
uint16_t bullet_42_remain;
uint16_t coin_remain;
uint16_t fortress_remain;
}Referee_BulletRemain_t;
typedef struct {
uint32_t rfid_status;
uint8_t rfid_status_2;
}Referee_RFID_t;
typedef struct __packed {
uint8_t dart_launch_opening_status;
uint8_t reserved;
uint16_t target_change_time;
uint16_t latest_launch_cmd_time;
}Referee_DartClient_t;
typedef struct {
float hero_x;
float hero_y;
float engineer_x;
float engineer_y;
float standard_3_x;
float standard_3_y;
float standard_4_x;
float standard_4_y;
float reserved_1;
float reserved_2;
} Referee_GroundRobotPos_t;
typedef struct __packed {
uint16_t mark_progress;
} Referee_RadarMark_t;
typedef struct __packed {
uint32_t sentry_info;
uint16_t sentry_info_2;
} Referee_SentryInfo_t;
typedef struct __packed {
uint8_t radar_info;
} Referee_RadarInfo_t;
typedef struct __packed {
uint16_t data_cmd_id;
uint16_t sender_id;
uint16_t receiver_id;
uint8_t user_data[REF_USER_DATA_MAX_LEN];
} Referee_RobotInteraction_t;
typedef struct __packed
{
uint8_t delete_type;
uint8_t layer;
}Referee_RobotInteractionLayerDelete_t;
typedef struct __packed {
uint8_t figure_name[3];
uint32_t operate_type:3;
uint32_t figure_type:3;
uint32_t layer:4;
uint32_t color:4;
uint32_t details_a:9;
uint32_t details_b:9;
uint32_t width:10;
uint32_t start_x:11;
uint32_t start_y:11;
uint32_t details_c:10;
uint32_t details_d:11;
uint32_t details_e:11;
} Referee_InteractionFigure_t;
typedef struct __packed {
Referee_InteractionFigure_t interaction_figure[2];
} Referee_InteractionFigure2_t;
typedef struct __packed {
Referee_InteractionFigure_t interaction_figure[5];
} Referee_InteractionFigure3_t;
typedef struct __packed {
Referee_InteractionFigure_t interaction_figure[7];
} Referee_InteractionFigure4_t;
typedef struct __packed {
//graphic_data_struct_t grapic_data_struct;
uint8_t data[30];
}Referee_ExtClientCustomCharacter_t;
typedef struct __packed {
uint32_t sentry_cmd;
} Referee_SentryCmd_t;
typedef struct __packed {
uint8_t radar_cmd;
uint8_t password_cmd;
uint8_t password_1;
uint8_t password_2;
uint8_t password_3;
uint8_t password_4;
uint8_t password_5;
uint8_t password_6;
} Referee_RadarCmd_t;
typedef struct __packed {
float target_position_x;
float target_position_y;
uint8_t cmd_keyboard;
uint8_t target_robot_id;
uint16_t cmd_source;
}Referee_MapCommand_t;
typedef struct __packed {
uint16_t hero_position_x;
uint16_t hero_position_y;
uint16_t engineer_position_x;
uint16_t engineer_position_y;
uint16_t infantry_3_position_x;
uint16_t infantry_3_position_y;
uint16_t infantry_4_position_x;
uint16_t infantry_4_position_y;
uint16_t infantry_5_position_x;
uint16_t infantry_5_position_y;
uint16_t sentry_position_x;
int16_t sentry_position_y;
} Referee_MapRobotData_t;
typedef struct
{
uint8_t intention;
uint16_t start_position_x;
uint16_t start_position_y;
int8_t delta_x[49];
int8_t delta_y[49];
uint16_t sender_id;
}Referee_MapData_t;
typedef struct
{
uint16_t sender_id;
uint16_t receiver_id;
uint8_t user_data[30];
}Referee_CustomInfo_t;
typedef struct
{
uint8_t data[30];
}Referee_CustomRobot_t;
typedef struct
{
uint8_t data[30];
}Referee_RobotCustom_t;
typedef struct
{
uint8_t data[30];
}Referee_RobotCustom2_t;
/* 适配 V1.2.0: 0x0311 新增指令 */
typedef struct __packed {
uint8_t data[30];
} Referee_RobotCustom3_t;
typedef struct __packed {
int16_t mouse_x;
int16_t mouse_y;
int16_t mouse_wheel;
int8_t button_l;
int8_t button_r;
uint16_t keyboard_value;
uint16_t res;
} Referee_KeyboardMouse_t;
typedef struct
{
uint16_t key_value;
uint16_t x_position:12;
uint16_t mouse_left:4;
uint16_t y_position:12;
uint16_t mouse_right:4;
uint16_t reserved;
}Referee_CustomClient_t;
typedef struct __packed {
uint8_t place_holder;
} Referee_InterStudent_Custom_t;
typedef struct __packed {
uint16_t ally_1_robot_HP;
uint16_t ally_2_robot_HP;
uint16_t ally_3_robot_HP;
uint16_t ally_4_robot_HP;
uint16_t reserved;
uint16_t ally_7_robot_HP;
} Referee_OppisiteGameRobotHP_t;
typedef struct __packed {
uint16_t hero_bullet;
uint16_t infantry3_bullet;
uint16_t infantry4_bullet;
uint16_t drone_bullet;
uint16_t sentry_bullet;
} Referee_OppsiteBulletRemain_t;
typedef struct __packed {
uint16_t remain_coins;
uint16_t total_coins;
uint32_t macro_status;
} Referee_OppsiteRobotState_t;
typedef struct __packed {
uint8_t data[36];
} Referee_OppsiteRobotBuff_t;
typedef struct __packed {
uint8_t data[6];
} Referee_OppsiteDisturbingWaveKey_t;
typedef struct __packed {
uint8_t f1_status : 1;
uint8_t f1_buff_status : 3;
uint8_t f2_status : 1;
uint8_t f2_buff_status : 3;
uint8_t f3_status : 1;
uint8_t f3_buff_status : 3;
uint8_t f4_status : 1;
uint8_t f4_buff_status : 3;
uint8_t f5_status : 1;
uint8_t f5_buff_status : 3;
uint8_t f6_status : 1;
uint8_t f6_buff_status : 3;
uint16_t red1_bullet_remain;
uint16_t red2_bullet_remain;
uint16_t blue1_bullet_remain;
uint16_t blue2_bullet_remain;
} Referee_ICRAZoneStatus_t;
typedef struct __packed {
uint8_t copter_pad : 2;
uint8_t energy_mech : 2;
uint8_t virtual_shield : 1;
uint32_t res : 27;
} Referee_FieldEvents_t;
typedef struct __packed {
uint8_t supply_id;
uint8_t robot_id;
uint8_t supply_step;
uint8_t supply_sum;
} Referee_SupplyAction_t;
typedef struct __packed {
uint8_t place_holder; /* TODO */
} Referee_RequestSupply_t;
typedef struct __packed {
uint8_t countdown;
} Referee_DartCountdown_t;
typedef struct __packed {
uint8_t attack_countdown;
} Referee_DroneEnergy_t;
/*typedef struct __packed {
uint8_t opening;
uint8_t target;
uint8_t target_changable_countdown;
uint8_t dart1_speed;
uint8_t dart2_speed;
uint8_t dart3_speed;
uint8_t dart4_speed;
uint16_t last_dart_launch_time;
uint16_t operator_cmd_launch_time;
} Referee_DartClient_t;*/
typedef struct __packed {
float position_x;
float position_y;
float position_z;
uint8_t commd_keyboard;
uint16_t robot_id;
} Referee_ClientMap_t;
typedef uint16_t Referee_Tail_t;
typedef enum {
REF_BOT_RED_HERO = 1,
REF_BOT_RED_ENGINEER = 2,
REF_BOT_RED_INFANTRY_1 = 3,
REF_BOT_RED_INFANTRY_2 = 4,
REF_BOT_RED_INFANTRY_3 = 5,
REF_BOT_RED_DRONE = 6,
REF_BOT_RED_SENTRY = 7,
REF_BOT_RED_RADER = 9,
REF_BOT_RED_OUTPOST=10,
REF_BOT_RED_BASE=11,
REF_BOT_BLU_HERO = 101,
REF_BOT_BLU_ENGINEER = 102,
REF_BOT_BLU_INFANTRY_1 = 103,
REF_BOT_BLU_INFANTRY_2 = 104,
REF_BOT_BLU_INFANTRY_3 = 105,
REF_BOT_BLU_DRONE = 106,
REF_BOT_BLU_SENTRY = 107,
REF_BOT_BLU_RADER = 109,
REF_BOT_BUL_OUTPOST=110,
REF_BOT_BUL_BASE=111,
} Referee_RobotID_t;
typedef enum {
REF_CL_RED_HERO = 0x0101,
REF_CL_RED_ENGINEER = 0x0102,
REF_CL_RED_INFANTRY_1 = 0x0103,
REF_CL_RED_INFANTRY_2 = 0x0104,
REF_CL_RED_INFANTRY_3 = 0x0105,
REF_CL_RED_DRONE = 0x0106,
REF_CL_BLU_HERO = 0x0165,
REF_CL_BLU_ENGINEER = 0x0166,
REF_CL_BLU_INFANTRY_1 = 0x0167,
REF_CL_BLU_INFANTRY_2 = 0x0168,
REF_CL_BLU_INFANTRY_3 = 0x0169,
REF_CL_BLU_DRONE = 0x016A,
} Referee_ClientID_t;
typedef enum {
REF_STDNT_CMD_ID_UI_DEL = 0x0100,
REF_STDNT_CMD_ID_UI_DRAW1 = 0x0101,
REF_STDNT_CMD_ID_UI_DRAW2 = 0x0102,
REF_STDNT_CMD_ID_UI_DRAW5 = 0x0103,
REF_STDNT_CMD_ID_UI_DRAW7 = 0x0104,
REF_STDNT_CMD_ID_UI_STR = 0x0110,
REF_STDNT_CMD_ID_CUSTOM = 0x0200,
} Referee_StudentCMDID_t;
typedef struct __packed {
Referee_StudentCMDID_t data_cmd_id;
uint16_t id_sender;
uint16_t id_receiver;
uint8_t *data;
} Referee_InterStudent_t;
typedef struct {
Referee_Status_t ref_status;
Referee_GameStatus_t game_status;
Referee_GameResult_t game_result;
Referee_GameRobotHP_t game_robot_hp;
Referee_Event_t event;
Referee_Warning_t warning;
Referee_DartInfo_t dartinfo;
Referee_RobotStatus_t robot_status;
Referee_PowerHeat_t power_heat;
Referee_RobotPos_t robot_pos;
Referee_RobotBuff_t robot_buff;
Referee_RobotDamage_t robot_danage;
Referee_ShootData_t shoot_data;
Referee_BulletRemain_t bullet_remain;
Referee_RFID_t rfid;
Referee_DartClient_t dart_client;
Referee_GroundRobotPos_t ground_robot_pos;
Referee_RadarMark_t radar_mark;
Referee_SentryInfo_t sentry_info;
Referee_RadarInfo_t radar_info;
Referee_RobotInteraction_t robot_interaction;
Referee_RobotInteractionLayerDelete_t robot_interaction_layer_delete;
Referee_InteractionFigure_t interaction_figure;
Referee_InteractionFigure2_t interaction_figure2;
Referee_InteractionFigure3_t interaction_figure3;
Referee_InteractionFigure4_t interaction_figure4;
Referee_ExtClientCustomCharacter_t ext_client_custom_character;
Referee_SentryCmd_t sentry_cmd;
Referee_RadarCmd_t radar_cmd;
Referee_MapCommand_t map_command;
Referee_MapRobotData_t map_robot_data;
Referee_MapData_t map_data;
Referee_CustomInfo_t custom_info;
Referee_CustomRobot_t custom_robot;
Referee_RobotCustom_t robot_custom;
Referee_RobotCustom2_t robot_custom2;
Referee_KeyboardMouse_t keyboard_mouse_t;
Referee_CustomClient_t custom_client;
Referee_MapRobotData_t map_oppsite_robot_data;
Referee_OppisiteGameRobotHP_t oppsite_robotHP;
Referee_OppsiteBulletRemain_t oppsite_bullet_remain;
Referee_OppsiteRobotState_t oppsite_robot_satate;
Referee_OppsiteRobotBuff_t oppsite_robot_buff;
Referee_OppsiteDisturbingWaveKey_t opsite_DisturbingWave_key;
Referee_ICRAZoneStatus_t icra_zone;
Referee_FieldEvents_t field_event;
Referee_SupplyAction_t supply_action;
Referee_RequestSupply_t request_supply;
Referee_DartCountdown_t dart_countdown;
Referee_DroneEnergy_t drone_energy;
Referee_InterStudent_Custom_t custom;
Referee_ClientMap_t client_map;
osTimerId_t ui_fast_timer_id;
osTimerId_t ui_slow_timer_id;
} Referee_t;
typedef struct __packed {
/* UI缓冲数据 */
UI_Ele_t grapic[REF_UI_MAX_GRAPIC_NUM];
UI_Drawcharacter_t character_data[REF_UI_MAX_STRING_NUM];
UI_Del_t del[REF_UI_MAX_DEL_NUM];
/* 待发送数量 */
uint8_t grapic_counter;
uint8_t character_counter;
uint8_t del_counter;
/* UI所需信息 */
Cap_RefereeUI_t cap_ui;
Chassis_RefereeUI_t chassis_ui;
Shoot_RefereeUI_t shoot_ui;
Gimbal_RefereeUI_t gimbal_ui;
bool cmd_pc;
/* 屏幕分辨率 */
const Referee_Screen_t *screen;
} Referee_UI_t;
typedef struct __packed {
uint16_t data_cmd_id;
uint16_t sender_ID;
uint16_t receiver_ID;
} Referee_Interactive_Header_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_1_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_1_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_2_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_2_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_5_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_5_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawgrapic_7_t data;
uint16_t crc16;
} Referee_UI_Drawgrapic_7_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Drawcharacter_t data;
uint16_t crc16;
} Referee_UI_Drawcharacter_t;
typedef struct __packed {
Referee_Header_t header;
uint16_t cmd_id;
Referee_Interactive_Header_t IA_header;
UI_Del_t data;
uint16_t crc16;
} Referee_UI_Del_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t Referee_Init(Referee_t *ref, Referee_UI_t *ui,
const Referee_Screen_t *screen);
int8_t Referee_Restart(void);
int8_t Referee_StartReceiving(Referee_t *ref);
int8_t Referee_Parse(Referee_t *ref);
void Referee_HandleOffline(Referee_t *referee);
int8_t Referee_SetHeader(Referee_Interactive_Header_t *header,
Referee_StudentCMDID_t cmd_id, uint8_t sender_id);
int8_t Referee_StartSend(uint8_t *data, uint32_t len);
int8_t Referee_MoveData(void *data, void *tmp, uint32_t len);
int8_t Referee_PackUI(Referee_UI_t *ui, Referee_t *ref);
UI_Ele_t *Referee_GetGrapicAdd(Referee_UI_t *ref_ui);
UI_Drawcharacter_t *Referee_GetCharacterAdd(Referee_UI_t *ref_ui);
uint8_t Referee_PraseCmd(Referee_UI_t *ref_ui, Referee_UI_CMD_t cmd);
uint8_t Referee_PackCap(Referee_ForCap_t *cap, const Referee_t *ref);
uint8_t Referee_PackShoot(Referee_ForShoot_t *ai, Referee_t *ref);
uint8_t Referee_PackChassis(Referee_ForChassis_t *chassis,
const Referee_t *ref);
uint8_t Referee_PackAI(Referee_ForAI_t *shoot, const Referee_t *ref);
uint8_t Referee_UIRefresh(Referee_UI_t *ui);
#ifdef __cplusplus
}
#endif

88
User/device/referee_proto_types.h Normal file
View File

@ -0,0 +1,88 @@
/*
* 裁判系统协议基础类型（叶节点头文件）
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
/* ---- 在线状态 ---- */
typedef enum { REF_STATUS_OFFLINE = 0, REF_STATUS_RUNNING } Referee_Status_t;
/* ---- 协议报文字段子集 ---- */
typedef struct __attribute__((packed)) {
uint8_t robot_id;
uint8_t robot_level;
uint16_t current_HP;
uint16_t maximum_HP;
uint16_t shooter_barrel_cooling_value;
uint16_t shooter_barrel_heat_limit;
uint16_t chassis_power_limit;
uint8_t power_management_gimbal_output : 1;
uint8_t power_management_chassis_output : 1;
uint8_t power_management_shooter_output : 1;
} Referee_RobotStatus_t;
typedef struct __attribute__((packed)) {
uint16_t chassis_volt;
uint16_t chassis_amp;
float chassis_watt;
uint16_t chassis_pwr_buff;
uint16_t shooter_17mm_barrel_heat;
uint16_t shooter_42mm_barrel_heat;
} Referee_PowerHeat_t;
typedef struct __attribute__((packed)) {
uint8_t bullet_type;
uint8_t shooter_id;
uint8_t bullet_freq;
float bullet_speed;
} Referee_ShootData_t;
/* ---- 转发包：各 module/task 消费 ---- */
typedef struct {
Referee_Status_t ref_status;
float chassis_watt;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForCap_t;
typedef struct {
Referee_Status_t ref_status;
} Referee_ForAI_t;
typedef struct {
Referee_Status_t ref_status;
float chassis_power_limit;
float chassis_pwr_buff;
} Referee_ForChassis_t;
typedef struct {
Referee_Status_t ref_status;
Referee_PowerHeat_t power_heat;
Referee_RobotStatus_t robot_status;
Referee_ShootData_t shoot_data;
} Referee_ForShoot_t;
/* ---- 屏幕分辨率 & UI 指令 ---- */
typedef struct {
uint16_t width;
uint16_t height;
} Referee_Screen_t;
typedef enum {
UI_NOTHING,
UI_AUTO_AIM_START,
UI_AUTO_AIM_STOP,
UI_HIT_SWITCH_START,
UI_HIT_SWITCH_STOP
} Referee_UI_CMD_t;
#ifdef __cplusplus
}
#endif

158
User/device/supercap.c Normal file
View File

@ -0,0 +1,158 @@
#include "device/supercap.h"
/* 全局变量 */
CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData = {0};
uint8_t PowerOffset =0;
uint32_t LastCapTick = 0; //上一次收到超电信号的时间戳
uint32_t NowCapTick = 0; //现在收到超电信号的时间戳
uint32_t chassis_energy_in_gamming =0;
/* 静态变量 - 用于CAN接收管理 */
static bool supercap_inited = false;
static osMessageQueueId_t supercap_rx_queue = NULL;
/**
* @brief 获取超级电容在线状态，1在线，0离线
*/
uint8_t get_supercap_online_state(void){
NowCapTick = HAL_GetTick(); //更新时间戳
uint32_t DeltaCapTick = 0;
DeltaCapTick = NowCapTick - LastCapTick; //计算时间差
// if(get_game_progress() == 4){
// //比赛开始的时候，开始统计消耗的能量
chassis_energy_in_gamming += CAN_SuperCapRXData.BatPower * DeltaCapTick *0.001f;
// 因为STM32的系统定时器是1ms的周期，所以*0.001，单位化为秒（S），能量单位才是焦耳（J）
// }
if(DeltaCapTick > 1000){
//如果时间差大于1s，说明超电信号丢失，返回超电离线的标志
return 0;
}else {
//如果时间差小于1s，说明超电信号正常，返回超电在线的标志
return 1;
}
}
/**
* @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J）
*/
uint32_t get_chassis_energy_from_supercap(void){
return chassis_energy_in_gamming;
}
/******************超级电容数据从CAN传到结构体******************/
int8_t SuperCap_Init(void)
{
if (supercap_inited) {
return DEVICE_OK; // 已经初始化过
}
if (BSP_FDCAN_RegisterId( SUPERCAP_CAN , SUPERCAP_RX_ID, 3) != BSP_OK) {
return DEVICE_ERR;
}
supercap_inited = true;
return DEVICE_OK;
}
int8_t SuperCap_Update(void)
{
if (!supercap_inited) {
return DEVICE_ERR;
}
BSP_FDCAN_Message_t rx_msg;
// 从CAN队列获取数据
if (BSP_FDCAN_GetMessage( SUPERCAP_CAN , SUPERCAP_RX_ID, &rx_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
// 处理接收到的数据
transfer_SuperCap_measure(rx_msg.data);
return DEVICE_OK;
}
return DEVICE_ERR; // 没有新数据
}
void transfer_SuperCap_measure(uint8_t *data)
{
LastCapTick = HAL_GetTick();
CAN_SuperCapRXData.SuperCapReady = (SuperCap_Status_t)data[0];
CAN_SuperCapRXData.SuperCapState = (SuperCap_Status_t)data[1];
CAN_SuperCapRXData.SuperCapEnergy = data[2];
CAN_SuperCapRXData.ChassisPower = data[3] << 1; //左移一位是为了扩大范围，超电发出来的的时候右移了一位
CAN_SuperCapRXData.BatVoltage = data[4];
CAN_SuperCapRXData.BatPower = data[5];
}
/**
* @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum
* @retval none
*/
SuperCapStateEnum get_supercap_state(void){
return (SuperCapStateEnum)CAN_SuperCapRXData.SuperCapState;
}
/**
* @brief 获取超级电容读到的电池电压，单位伏（V）
* @retval none
*/
float get_battery_voltage_from_supercap(void){
return (float)CAN_SuperCapRXData.BatVoltage * 0.1f;
}
/**
* @brief 获取超级电容可用能量，范围：0-100%
* @retval none
*/
uint8_t get_supercap_energy(void){
return CAN_SuperCapRXData.SuperCapEnergy;
}
/**
* @brief 设置超级电容功率补偿
* @retval none
*/
void set_supercap_power_offset(uint8_t offset){
PowerOffset = offset;
}
/**
* @brief 发送超级电容数据
* @param[in] Enable: 超级电容使能
* @param[in] Charge: 超级电容充电，在PLUS版本中无效
* @param[in] PowerLimit: 裁判系统功率限制
* @param[in] Chargepower: 超级电容充电功率，在PLUS版本中无效
* @retval none
*/
int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp)
{
if (TX_Temp == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_StdDataFrame_t tx_frame;
tx_frame.id = SUPERCAP_TX_ID;
tx_frame.dlc = 0x08;
tx_frame.data[0] = TX_Temp-> Enable;
tx_frame.data[1] = TX_Temp-> Charge;//PLUS disabled
tx_frame.data[2] = TX_Temp-> Powerlimit - PowerOffset;
tx_frame.data[3] = TX_Temp-> ChargePower;//PLUS disabled
tx_frame.data[4] = 0;
tx_frame.data[5] = 0;
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
return BSP_FDCAN_TransmitStdDataFrame( SUPERCAP_CAN , &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

108
User/device/supercap.h Normal file
View File

@ -0,0 +1,108 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "bsp/fdcan.h"
#include "device/device.h"
//#include "referee.h"
#define SUPERCAP_CAN BSP_FDCAN_1
//#define SUPERCAP_CAN BSP_FDCAN_2
#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID
#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID
//超级电容的状态标志位，超级电容运行或者保护的具体状态反馈
typedef enum
{
DISCHARGE =0 , //放电状态
CHARGE =1, //充电状态
WAIT =2, //待机状态
SOFTSTART_PROTECTION =3,//处于软起动状态
OVER_LOAD_PROTECTION = 4, //超电过载保护状态
BAT_OVER_VOLTAGE_PROTECTION =5, //过压保护状态
BAT_UNDER_VOLTAGE_PROTECTION =6, //电池欠压保护，电池要没电了，换电池
CAP_UNDER_VOLTAGE_PROTECTION =7, //超级电容欠压保护，超级电容用完电了，要充一会才能用
OVER_TEMPERATURE_PROTECTION =8, //过温保护，太热了
BOOM = 9, //超电爆炸了
}SuperCapStateEnum;
//超级电容准备状态，用于判断超级电容是否可以使用
typedef enum
{
SUPERCAP_STATUS_OFFLINE =0 ,
SUPERCAP_STATUS_RUNNING =1,
}SuperCap_Status_t;
// 发送给超级电容的数据
typedef struct {
FunctionalState Enable ; //超级电容使能。1使能，0失能
SuperCapStateEnum Charge ; //V1.1超级电容充电。1充电，0放电，在PLUS版本中，此标志位无效，超电的充放电是自动的
uint8_t Powerlimit; //裁判系统功率限制
uint8_t ChargePower; //V1.1超级电容充电功率，在PLUS版本中，此参数，超电的充电功率随着底盘功率变化
}CAN_SuperCapTXDataTypeDef;
// 从超级电容接收到的数据
typedef struct {
uint8_t SuperCapEnergy;//超级电容可用能量：0-100%
uint16_t ChassisPower; //底盘功率，0-512，由于传输的时候为了扩大量程右移了一位，所以接收的时候需要左移还原（丢精度）。
SuperCap_Status_t SuperCapReady;//超级电容【可用标志】：1为可用，0为不可用
SuperCapStateEnum SuperCapState;//超级电容【状态标志】：各个状态对应的状态码查看E_SuperCapState枚举。
uint8_t BatVoltage; //通过超级电容监控电池电压*10，
uint8_t BatPower;
}CAN_SuperCapRXDataTypeDef;
extern CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData;
void set_supercap_power_offset(uint8_t offset);
// 以下函数是超电控制所需要调用的函数
void transfer_SuperCap_measure( uint8_t *data);
int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp);
/**
* @brief 获取超级电容在线状态，1在线，0离线
*/
uint8_t get_supercap_online_state(void);
/**
* @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum
*/
SuperCapStateEnum get_supercap_state(void);
/**
* @brief 获取超级电容读到的电池电压，单位伏（V）
*/
float get_battery_voltage_from_supercap(void);
/**
* @brief 获取超级电容可用能量，范围：0-100%
*/
uint8_t get_supercap_energy(void);
/**
* @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J）
*/
uint32_t get_chassis_energy_from_supercap(void);
int8_t SuperCap_Init(void);
int8_t SuperCap_Update(void);
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
float percentage;
SuperCap_Status_t status;
} Cap_RefereeUI_t;
#ifdef __cplusplus
}
#endif /*SUPERCAP_H*/

12
User/module/balance_chassis.c
View File

@ -1098,3 +1098,15 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) {
return CHASSIS_OK;
}
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui) {
ui->mode = c->mode;
// ui->angle = c->feedback.yaw.rotor_abs_angle - c->mech_zero;
// #error "右边那个mech_zero应该是跟随云台的那个角,我没找着在哪"
}

7
User/module/balance_chassis.h
View File

@ -87,6 +87,11 @@ typedef struct {
float wheel[2]; /* 两个轮子电机输出 */
}Chassis_Output_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
Chassis_Mode_t mode;
float angle;
} Chassis_RefereeUI_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
@ -294,6 +299,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
*/
void Chassis_Output(Chassis_t *c);
void Chassis_DumpUI(const Chassis_t *c, Chassis_RefereeUI_t *ui);
#ifdef __cplusplus
}
#endif

744
User/module/cmd/cmd.c Normal file
View File

@ -0,0 +1,744 @@
/*
* CMD 模块 V2 - 主控制模块实现
*/
#include "cmd.h"
#include "bsp/time.h"
#include <stdint.h>
#include <string.h>
/* ========================================================================== */
/* 命令构建函数 */
/* ========================================================================== */
/* 从RC输入生成底盘命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
/* 根据左拨杆位置选择模式 */
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.chassis.cmd.mode = map->sw_left_up;
break;
case CMD_SW_MID:
ctx->output.chassis.cmd.mode = map->sw_left_mid;
break;
case CMD_SW_DOWN:
ctx->output.chassis.cmd.mode = map->sw_left_down;
break;
default:
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
break;
}
/* 摇杆控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x;
ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y;
ctx->output.chassis.cmd.ctrl_vec.wz = ctx->input.rc.joy_left.x;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */
/* 从 RC 输入生成云台命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
/* 根据拨杆选择云台模式 */
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_up;
break;
case CMD_SW_MID:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_mid;
break;
case CMD_SW_DOWN:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_down;
break;
default:
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
break;
}
/* 左摇杆控制云台 */
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_right.x * 5.0f;
ctx->output.gimbal.cmd.delta_pit = ctx->input.rc.joy_right.y * 5.0f;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */
/* 从 RC 输入生成射击命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT
static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
if (ctx->input.online[CMD_SRC_RC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST;
} else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
}
/* 根据右拨杆控制射击 */
switch (ctx->input.rc.sw[1]) {
case CMD_SW_DOWN:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = true;
break;
case CMD_SW_MID:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case CMD_SW_UP:
default:
ctx->output.shoot.cmd.ready = false;
ctx->output.shoot.cmd.firecmd = false;
break;
}
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */
/* 从 RC 输入生成履带命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK
static void CMD_RC_BuildTrackCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
if (!ctx->input.online[CMD_SRC_RC]) {
ctx->output.track.cmd.enable = false;
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
return;
}
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.track.cmd.enable = map->track_sw_up;
break;
case CMD_SW_MID:
ctx->output.track.cmd.enable = map->track_sw_mid;
break;
case CMD_SW_DOWN:
ctx->output.track.cmd.enable = map->track_sw_down;
break;
default:
ctx->output.track.cmd.enable = false;
break;
}
ctx->output.track.cmd.enable = ctx->input.online[CMD_SRC_RC];
ctx->output.track.cmd.vel = ctx->input.rc.joy_right.y * 2.0f;
if(fabsf(ctx->input.rc.joy_right.y * 2.0f) > 1.0f)
ctx->output.track.cmd.vel = ctx->output.track.cmd.vel > 0 ? 1.0f
: -1.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK */
/* 从PC输入生成底盘命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
return;
}
ctx->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
/* WASD控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f;
ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */
/* 从PC输入生成云台命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
return;
}
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
/* 鼠标控制云台 */
ctx->output.gimbal.cmd.delta_yaw = (float)-ctx->input.pc.mouse.x * ctx->timer.dt * sens->mouse_sens;
ctx->output.gimbal.cmd.delta_pit = (float)ctx->input.pc.mouse.y * ctx->timer.dt * sens->mouse_sens * 1.5f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_GIMBAL);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL */
/* 从PC输入生成射击命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT
static void CMD_PC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
return;
}
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = ctx->input.pc.mouse.l_click;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */
/* 从PC输入生成履带命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK
static void CMD_PC_BuildTrackCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.track.cmd.enable = false;
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
return;
}
ctx->output.track.cmd.enable = true;
/* 可根据需要添加PC对履带的控制，例如键盘按键 */
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK */
/* 从NUC/AI输入生成云台命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
return;
}
/* 使用AI提供的云台控制数据 */
if (ctx->input.nuc.mode!=0) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_AI_CONTROL;
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
}
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */
/* 从 NUC/AI 输入生成射击命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT
static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
return;
}
/* 根据AI模式决定射击行为 */
switch (ctx->input.nuc.mode) {
case 0:
ctx->output.shoot.cmd.ready = false;
ctx->output.shoot.cmd.firecmd = false;
break;
case 1:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case 2:
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = true;
}else {
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
}
break;
}
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */
/* 从 RC 输入生成机械臂命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM
static void CMD_RC_BuildArmCmd(CMD_t *ctx) {
/*
* 遥控器控制机械臂末端位姿 —— 笛卡尔增量方案
*
* 左拨杆 SW_L (sw[0]) —— 整体使能:
* UP → 失能（电机松弛/重力补偿）
* MID → 使能，笛卡尔增量控制
* DOWN → 使能（保留备用）
*
* 右拨杆 SW_R (sw[1]) —— 自由度分组:
*
* UP 【位置模式】3轴平移 + 偏航（最常用操作放第一层）
* 右摇杆X (RX) → X 平移（末端左右移动）
* 右摇杆Y (RY) → Y 平移（末端前后移动）
* 左摇杆Y (LY) → Z 平移（末端升降）
* 左摇杆X (LX) → Yaw 偏航旋转
*
* MID 【姿态模式】俯仰/横滚全姿态，同时保留 Z 和偏航可调
* 右摇杆X (RX) → Yaw 偏航旋转（持续可调，避免强制切换模式）
* 右摇杆Y (RY) → Pitch 俯仰旋转
* 左摇杆X (LX) → Roll 横滚旋转
* 左摇杆Y (LY) → Z 平移（升降保持可调，避免强制切换模式）
*
* DOWN → set_target_as_current（目标位姿吸附当前实际位姿，消除累积漂移）
*
* 摇杆直觉映射总结:
* 右摇杆 = "末端去哪里"（XY平移，最自然）
* 左Y = "臂的高低" （Z升降，推上=升高）
* 左X = 位置模式→偏航 / 姿态模式→横滚
*/
ctx->output.arm.cmd.set_target_as_current = false;
if (ctx->input.rc.sw[1] == CMD_SW_DOWN) {
ctx->output.arm.cmd.set_target_as_current = true;
}
switch (ctx->input.rc.sw[0]) {
case CMD_SW_MID:
case CMD_SW_DOWN:
ctx->output.arm.cmd.enable = true;
break;
default:
ctx->output.arm.cmd.enable = false;
goto end;
}
/* 遥控器模式使用笛卡尔位姿累积控制 */
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
/* set_target_as_current 置位时不叠加摇杆增量，由上层负责同步位姿基准 */
if (ctx->output.arm.cmd.set_target_as_current) return;
/* 输出摇杆速度命令（m/s, rad/s）——不含 dt，由arm_main在正确坐标系下积分到世界系 target_pose */
float pos_scale = 0.3f; /* 末端线速度上限 (m/s) */
float rot_scale = 1.0f; /* 末端角速度上限 (rad/s) */
memset(&ctx->output.arm.cmd.joy_vel, 0, sizeof(ctx->output.arm.cmd.joy_vel));
switch (ctx->input.rc.sw[1]) {
case CMD_SW_UP:
/* 位置模式：3轴平移 + 偏航 */
ctx->output.arm.cmd.joy_vel.x = ctx->input.rc.joy_right.x * pos_scale;
ctx->output.arm.cmd.joy_vel.y = ctx->input.rc.joy_right.y * pos_scale;
ctx->output.arm.cmd.joy_vel.z = ctx->input.rc.joy_left.y * pos_scale;
ctx->output.arm.cmd.joy_vel.yaw = ctx->input.rc.joy_left.x * rot_scale;
break;
case CMD_SW_MID:
/* 姿态模式：俯仰 + 横滚 + 偏航 + 升降（全6自由度可达，Z/Yaw持续可调） */
ctx->output.arm.cmd.joy_vel.yaw = ctx->input.rc.joy_right.x * rot_scale;
ctx->output.arm.cmd.joy_vel.pitch = ctx->input.rc.joy_right.y * rot_scale;
ctx->output.arm.cmd.joy_vel.roll = ctx->input.rc.joy_left.x * rot_scale;
ctx->output.arm.cmd.joy_vel.z = ctx->input.rc.joy_left.y * pos_scale;
break;
default:
break;
}
end:
return;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_ARM */
/* 从 PC 输入生成机械臂命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_ARM
static void CMD_PC_BuildArmCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.arm.cmd.enable = false;
return;
}
ctx->output.arm.cmd.enable = true;
ctx->output.arm.cmd.ctrl_type = ARM_CTRL_REMOTE_CARTESIAN;
/* 鼠标控制末端位坐 XY，其他轴可根据需要扩展 */
float sens = ctx->config->sensitivity.mouse_sens * 0.001f;
ctx->output.arm.cmd.target_pose.x += (float)ctx->input.pc.mouse.x * sens * ctx->timer.dt;
ctx->output.arm.cmd.target_pose.y += (float)ctx->input.pc.mouse.y * sens * ctx->timer.dt;
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_ARM */
/* 从 RC 输入生成平衡底盘命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_BALANCE_CHASSIS
static void CMD_RC_BuildBalanceChassisCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.balance_chassis.cmd.mode = map->balance_sw_up;
break;
case CMD_SW_MID:
ctx->output.balance_chassis.cmd.mode = map->balance_sw_mid;
break;
case CMD_SW_DOWN:
ctx->output.balance_chassis.cmd.mode = map->balance_sw_down;
break;
default:
ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_RELAX;
break;
}
/* 左摩杆Y/X → 平移，右摩杆X → 转向 */
ctx->output.balance_chassis.cmd.move_vec.vx = ctx->input.rc.joy_left.y;
ctx->output.balance_chassis.cmd.move_vec.vy = ctx->input.rc.joy_left.x;
ctx->output.balance_chassis.cmd.move_vec.wz = ctx->input.rc.joy_right.x;
/* 拨轮传递目标高度 */
ctx->output.balance_chassis.cmd.height = ctx->input.rc.dial;
ctx->output.balance_chassis.cmd.jump_trigger = false;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */
/* 从 PC 输入生成平衡底盘命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS
static void CMD_PC_BuildBalanceChassisCmd(CMD_t *ctx) {
CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_RELAX;
return;
}
ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
float vx = 0.0f, wz = 0.0f;
uint32_t kb = ctx->input.pc.keyboard.bitmap;
//案件的命令要放到behavior里，
// if (kb & CMD_KEY_W) vx += sens->move_sens;
// if (kb & CMD_KEY_S) vx -= sens->move_sens;
// if (kb & CMD_KEY_A) wz += sens->move_sens;
// if (kb & CMD_KEY_D) wz -= sens->move_sens;
// if (kb & CMD_KEY_SHIFT) { vx *= sens->move_fast_mult; wz *= sens->move_fast_mult; }
// if (kb & CMD_KEY_CTRL) { vx *= sens->move_slow_mult; wz *= sens->move_slow_mult; }
ctx->output.balance_chassis.cmd.move_vec.vx = vx;
ctx->output.balance_chassis.cmd.move_vec.wz = wz;
ctx->output.balance_chassis.cmd.move_vec.vy = 0.0f;
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_BALANCE_CHASSIS */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_REFUI
static void CMD_NUC_BuildRefUICmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.refui.cmd = UI_AUTO_AIM_STOP;
return;
}
ctx->output.refui.cmd = (ctx->input.nuc.mode != 0) ? UI_AUTO_AIM_START : UI_AUTO_AIM_STOP;
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_REFUI */
/* 离线安全模式 */
static void CMD_SetOfflineMode(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.cmd.enable = false;
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
ctx->output.balance_chassis.cmd.mode = CHASSIS_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_ARM
ctx->output.arm.cmd.enable = false;
#endif
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.cmd = UI_NOTHING;
#endif
}
/* ========================================================================== */
/* 公开API实现 */
/* ========================================================================== */
int8_t CMD_Init(CMD_t *ctx, CMD_Config_t *config) {
if (ctx == NULL || config == NULL) {
return CMD_ERR_NULL;
}
memset(ctx, 0, sizeof(CMD_t));
ctx->config = config;
/* 初始化适配器 */
CMD_Adapter_InitAll();
/* 初始化行为处理器 */
CMD_Behavior_Init();
return CMD_OK;
}
int8_t CMD_UpdateInput(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* 保存上一帧输入 */
memcpy(&ctx->last_input, &ctx->input, sizeof(ctx->input));
/* 更新所有输入源 */
for (int i = 0; i < CMD_SRC_NUM; i++) {
CMD_Adapter_GetInput((CMD_InputSource_t)i, &ctx->input);
}
return CMD_OK;
}
typedef void (*CMD_BuildCommandFunc)(CMD_t *cmd);
typedef struct {
CMD_InputSource_t source;
CMD_BuildCommandFunc chassisFunc;
CMD_BuildCommandFunc gimbalFunc;
CMD_BuildCommandFunc shootFunc;
CMD_BuildCommandFunc trackFunc;
CMD_BuildCommandFunc armFunc;
CMD_BuildCommandFunc refuiFunc;
CMD_BuildCommandFunc balanceChassisFunc;
} CMD_SourceHandler_t;
/* Build-function conditional references */
#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_RC
#define _FN_RC_CHASSIS CMD_RC_BuildChassisCmd
#else
#define _FN_RC_CHASSIS NULL
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_RC
#define _FN_RC_GIMBAL CMD_RC_BuildGimbalCmd
#else
#define _FN_RC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_RC
#define _FN_RC_SHOOT CMD_RC_BuildShootCmd
#else
#define _FN_RC_SHOOT NULL
#endif
#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_RC
#define _FN_RC_TRACK CMD_RC_BuildTrackCmd
#else
#define _FN_RC_TRACK NULL
#endif
#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_PC
#define _FN_PC_CHASSIS CMD_PC_BuildChassisCmd
#else
#define _FN_PC_CHASSIS NULL
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_PC
#define _FN_PC_GIMBAL CMD_PC_BuildGimbalCmd
#else
#define _FN_PC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_PC
#define _FN_PC_SHOOT CMD_PC_BuildShootCmd
#else
#define _FN_PC_SHOOT NULL
#endif
#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_PC
#define _FN_PC_TRACK CMD_PC_BuildTrackCmd
#else
#define _FN_PC_TRACK NULL
#endif
#if CMD_ENABLE_MODULE_ARM && CMD_ENABLE_SRC_RC
#define _FN_RC_ARM CMD_RC_BuildArmCmd
#else
#define _FN_RC_ARM NULL
#endif
#if CMD_ENABLE_MODULE_ARM && CMD_ENABLE_SRC_PC
#define _FN_PC_ARM CMD_PC_BuildArmCmd
#else
#define _FN_PC_ARM NULL
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS && CMD_ENABLE_SRC_RC
#define _FN_RC_BALANCE_CHASSIS CMD_RC_BuildBalanceChassisCmd
#else
#define _FN_RC_BALANCE_CHASSIS NULL
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS && CMD_ENABLE_SRC_PC
#define _FN_PC_BALANCE_CHASSIS CMD_PC_BuildBalanceChassisCmd
#else
#define _FN_PC_BALANCE_CHASSIS NULL
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_NUC
#define _FN_NUC_GIMBAL CMD_NUC_BuildGimbalCmd
#else
#define _FN_NUC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_NUC
#define _FN_NUC_SHOOT CMD_NUC_BuildShootCmd
#else
#define _FN_NUC_SHOOT NULL
#endif
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
#if CMD_ENABLE_SRC_RC
[CMD_SRC_RC] = {CMD_SRC_RC, _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK, _FN_RC_ARM, NULL, _FN_RC_BALANCE_CHASSIS},
#endif
#if CMD_ENABLE_SRC_PC
[CMD_SRC_PC] = {CMD_SRC_PC, _FN_PC_CHASSIS, _FN_PC_GIMBAL, _FN_PC_SHOOT, _FN_PC_TRACK, _FN_PC_ARM, NULL, _FN_PC_BALANCE_CHASSIS},
#endif
#if CMD_ENABLE_SRC_NUC
#if CMD_ENABLE_MODULE_REFUI
#define _FN_NUC_REFUI CMD_NUC_BuildRefUICmd
#else
#define _FN_NUC_REFUI NULL
#endif
[CMD_SRC_NUC] = {CMD_SRC_NUC, NULL, _FN_NUC_GIMBAL, _FN_NUC_SHOOT, NULL, NULL, _FN_NUC_REFUI, NULL},
#endif
#if CMD_ENABLE_SRC_REF
[CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL, NULL, NULL, NULL}
#endif
};
int8_t CMD_Arbitrate(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* RC > PC priority arbitration */
CMD_InputSource_t candidates[] = {
#if CMD_ENABLE_SRC_RC
CMD_SRC_RC,
#endif
#if CMD_ENABLE_SRC_PC
CMD_SRC_PC,
#endif
};
const int num_candidates = sizeof(candidates) / sizeof(candidates[0]);
/* keep current source if still online */
if (ctx->active_source < CMD_SRC_NUM &&
#if CMD_ENABLE_SRC_REF
ctx->active_source != CMD_SRC_REF &&
#endif
ctx->input.online[ctx->active_source]) {
goto seize;
}
/* 否则选择第一个可用的控制输入源 */
for (int i = 0; i < num_candidates; i++) {
CMD_InputSource_t src = candidates[i];
if (ctx->input.online[src]) {
ctx->active_source = src;
break;
}else {
ctx->active_source = CMD_SRC_NUM;
continue;
}
}
/* 优先级抢占逻辑 */
seize:
/* reset output sources */
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_ARM
ctx->output.arm.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
ctx->output.balance_chassis.source = ctx->active_source;
#endif
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = CMD_SRC_NUC;
#endif
}
}
}
#endif
return CMD_OK;
}
int8_t CMD_GenerateCommands(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* 更新时间 */
uint64_t now_us = BSP_TIME_Get_us();
ctx->timer.now = now_us / 1000000.0f;
ctx->timer.dt = (now_us - ctx->timer.last_us) / 1000000.0f;
ctx->timer.last_us = now_us;
/* 没有有效输入源 */
if (ctx->active_source >= CMD_SRC_NUM) {
CMD_SetOfflineMode(ctx);
return CMD_ERR_NO_INPUT;
}
#if CMD_ENABLE_MODULE_GIMBAL
if (sourceHandlers[ctx->output.gimbal.source].gimbalFunc != NULL) {
sourceHandlers[ctx->output.gimbal.source].gimbalFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_CHASSIS
if (sourceHandlers[ctx->output.chassis.source].chassisFunc != NULL) {
sourceHandlers[ctx->output.chassis.source].chassisFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_SHOOT
if (sourceHandlers[ctx->output.shoot.source].shootFunc != NULL) {
sourceHandlers[ctx->output.shoot.source].shootFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_TRACK
if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) {
sourceHandlers[ctx->output.track.source].trackFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_ARM
if (sourceHandlers[ctx->output.arm.source].armFunc != NULL) {
sourceHandlers[ctx->output.arm.source].armFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_REFUI
if (sourceHandlers[ctx->output.refui.source].refuiFunc != NULL) {
sourceHandlers[ctx->output.refui.source].refuiFunc(ctx);
} else {
ctx->output.refui.cmd = UI_NOTHING;
}
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
if (sourceHandlers[ctx->output.balance_chassis.source].balanceChassisFunc != NULL) {
sourceHandlers[ctx->output.balance_chassis.source].balanceChassisFunc(ctx);
}
#endif
return CMD_OK;
}
#if CMD_ENABLE_SRC_REF
static void CMD_REF_BuildOutput(CMD_t *ctx) {
ctx->output.ref = ctx->input.ref;
}
#endif
int8_t CMD_Update(CMD_t *ctx) {
int8_t ret;
ret = CMD_UpdateInput(ctx);
if (ret != CMD_OK) return ret;
CMD_Arbitrate(ctx);
ret = CMD_GenerateCommands(ctx);
#if CMD_ENABLE_SRC_REF
CMD_REF_BuildOutput(ctx);
#endif
return ret;
}

306
User/module/cmd/cmd.h Normal file
View File

@ -0,0 +1,306 @@
/*
* CMD 模块 V2 - 主控制模块
* 统一的命令控制接口
*/
#pragma once
#include "cmd_types.h"
#include "cmd_adapter.h"
#include "cmd_behavior.h"
/* 按需引入输出模块的命令类型 */
#if CMD_ENABLE_MODULE_CHASSIS
#include "module/chassis.h"
#endif
#if CMD_ENABLE_MODULE_GIMBAL
#include "module/gimbal.h"
#endif
#if CMD_ENABLE_MODULE_SHOOT
#include "module/shoot.h"
#endif
#if CMD_ENABLE_MODULE_TRACK
#include "module/track.h"
#endif
#if CMD_ENABLE_MODULE_ARM
#include "component/arm_kinematics/arm6dof.h"
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
#include "module/balance_chassis.h"
#endif
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
/* ========================================================================== */
/* 输出命令结构 */
/* ========================================================================== */
/* 每个模块的输出包含源信息和命令 */
#if CMD_ENABLE_MODULE_CHASSIS
typedef struct {
CMD_InputSource_t source;
Chassis_CMD_t cmd;
} CMD_ChassisOutput_t;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
typedef struct {
CMD_InputSource_t source;
Gimbal_CMD_t cmd;
} CMD_GimbalOutput_t;
#endif
#if CMD_ENABLE_MODULE_SHOOT
typedef struct {
CMD_InputSource_t source;
Shoot_CMD_t cmd;
} CMD_ShootOutput_t;
#endif
#if CMD_ENABLE_MODULE_TRACK
typedef struct {
CMD_InputSource_t source;
Track_CMD_t cmd;
} CMD_TrackOutput_t;
#endif
#if CMD_ENABLE_MODULE_ARM
typedef struct {
CMD_InputSource_t source;
Arm_CMD_t cmd;
} CMD_ArmOutput_t;
#endif
#if CMD_ENABLE_MODULE_REFUI
typedef struct {
CMD_InputSource_t source;
Referee_UI_CMD_t cmd;
} CMD_RefUIOutput_t;
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
typedef struct {
CMD_InputSource_t source;
Chassis_CMD_t cmd;
} CMD_BalanceChassisOutput_t;
#endif
#if CMD_ENABLE_SRC_REF
// /* REF 透传输出：裁判数据直通各模块，不参与仲裁 */
// typedef CMD_RawInput_REF_t CMD_RawInput_REF_t;
#endif
/* ========================================================================== */
/* 配置结构 */
/* ========================================================================== */
/* 灵敏度配置 */
typedef struct {
float mouse_sens; /* 鼠标灵敏度 */
float move_sens; /* 移动灵敏度 */
float move_fast_mult; /* 快速移动倍率 */
float move_slow_mult; /* 慢速移动倍率 */
} CMD_Sensitivity_t;
/* RC模式映射配置 - 定义开关位置到模式的映射 */
typedef struct {
#if CMD_ENABLE_MODULE_CHASSIS
/* 左拨杆映射 - 底盘模式 */
Chassis_Mode_t sw_left_up;
Chassis_Mode_t sw_left_mid;
Chassis_Mode_t sw_left_down;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
/* 右拨杆映射 - 云台/射击模式 */
Gimbal_Mode_t gimbal_sw_up;
Gimbal_Mode_t gimbal_sw_mid;
Gimbal_Mode_t gimbal_sw_down;
#endif
#if CMD_ENABLE_MODULE_TRACK
/* 左拨杆映射 - 履带使能 */
bool track_sw_up;
bool track_sw_mid;
bool track_sw_down;
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
/* 左拨杆映射 - 平衡底盘模式 */
Chassis_Mode_t balance_sw_up;
Chassis_Mode_t balance_sw_mid;
Chassis_Mode_t balance_sw_down;
#endif
} CMD_RCModeMap_t;
/* 整体配置 */
typedef struct {
/* 输入源优先级，索引越小优先级越高 */
CMD_InputSource_t source_priority[CMD_SRC_NUM];
/* 灵敏度设置 */
CMD_Sensitivity_t sensitivity;
/* RC模式映射 */
CMD_RCModeMap_t rc_mode_map;
} CMD_Config_t;
/* ========================================================================== */
/* 主控制上下文 */
/* ========================================================================== */
typedef struct {
float now;
float dt;
uint32_t last_us;
} CMD_Timer_t;
typedef struct CMD_Context {
/* 配置 */
CMD_Config_t *config;
/* 时间 */
CMD_Timer_t timer;
/* 当前帧和上一帧的原始输入 */
CMD_RawInput_t input;
CMD_RawInput_t last_input;
/* 仲裁后的活跃输入源 */
CMD_InputSource_t active_source;
/* 输出 */
struct {
#if CMD_ENABLE_MODULE_CHASSIS
CMD_ChassisOutput_t chassis;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
CMD_GimbalOutput_t gimbal;
#endif
#if CMD_ENABLE_MODULE_SHOOT
CMD_ShootOutput_t shoot;
#endif
#if CMD_ENABLE_MODULE_TRACK
CMD_TrackOutput_t track;
#endif
#if CMD_ENABLE_MODULE_ARM
CMD_ArmOutput_t arm;
#endif
#if CMD_ENABLE_MODULE_REFUI
CMD_RefUIOutput_t refui;
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
CMD_BalanceChassisOutput_t balance_chassis;
#endif
#if CMD_ENABLE_SRC_REF
CMD_RawInput_REF_t ref;
#endif
} output;
} CMD_t;
/* ========================================================================== */
/* 主API接口 */
/* ========================================================================== */
/**
* @brief 初始化CMD模块
* @param ctx CMD上下文
* @param config 配置指针
* @return CMD_OK成功，其他失败
*/
int8_t CMD_Init(CMD_t *ctx, CMD_Config_t *config);
/**
* @brief 更新所有输入源的数据
* @param ctx CMD上下文
* @return CMD_OK成功
*/
int8_t CMD_UpdateInput(CMD_t *ctx);
/**
* @brief 执行仲裁，决定使用哪个输入源
* @param ctx CMD上下文
* @return 选中的输入源
*/
int8_t CMD_Arbitrate(CMD_t *ctx);
/**
* @brief 生成所有模块的控制命令
* @param ctx CMD上下文
* @return CMD_OK成功
*/
int8_t CMD_GenerateCommands(CMD_t *ctx);
/**
* @brief 一键更新（包含UpdateInput + Arbitrate + GenerateCommands）
* @param ctx CMD上下文
* @return CMD_OK成功
*/
int8_t CMD_Update(CMD_t *ctx);
/* ========================================================================== */
/* 输出获取接口 */
/* ========================================================================== */
/* 获取底盘命令 */
#if CMD_ENABLE_MODULE_CHASSIS
static inline Chassis_CMD_t* CMD_GetChassisCmd(CMD_t *ctx) {
return &ctx->output.chassis.cmd;
}
#endif
/* 获取云台命令 */
#if CMD_ENABLE_MODULE_GIMBAL
static inline Gimbal_CMD_t* CMD_GetGimbalCmd(CMD_t *ctx) {
return &ctx->output.gimbal.cmd;
}
#endif
/* 获取射击命令 */
#if CMD_ENABLE_MODULE_SHOOT
static inline Shoot_CMD_t* CMD_GetShootCmd(CMD_t *ctx) {
return &ctx->output.shoot.cmd;
}
#endif
/* 获取履带命令 */
#if CMD_ENABLE_MODULE_TRACK
static inline Track_CMD_t* CMD_GetTrackCmd(CMD_t *ctx) {
return &ctx->output.track.cmd;
}
#endif
/* 获取机械臂命令 */
#if CMD_ENABLE_MODULE_ARM
static inline Arm_CMD_t* CMD_GetArmCmd(CMD_t *ctx) {
return &ctx->output.arm.cmd;
}
#endif
/* 获取裁判系UI命令 */
#if CMD_ENABLE_MODULE_REFUI
static inline Referee_UI_CMD_t* CMD_GetRefUICmd(CMD_t *ctx) {
return &ctx->output.refui.cmd;
}
#endif
/* 获取平衡底盘命令 */
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
static inline Chassis_CMD_t* CMD_GetBalanceChassisCmd(CMD_t *ctx) {
return &ctx->output.balance_chassis.cmd;
}
#endif
/* 获取裁判系透传数据 */
#if CMD_ENABLE_SRC_REF
static inline CMD_RawInput_REF_t* CMD_GetRefData(CMD_t *ctx) {
return &ctx->output.ref;
}
#endif
#ifdef __cplusplus
}
#endif

258
User/module/cmd/cmd_adapter.c Normal file
View File

@ -0,0 +1,258 @@
/*
* CMD 模块 V2 - 输入适配器实现
*/
#include "cmd_adapter.h"
#include "module/cmd/cmd_adapter.h"
#include <stdbool.h>
#include <string.h>
/* ========================================================================== */
/* 适配器存储 */
/* ========================================================================== */
// static CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0};
CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0};
/* ========================================================================== */
/* DR16 适配器实现 */
/* ========================================================================== */
#if CMD_RC_DEVICE_TYPE == 0
int8_t CMD_DR16_Init(void *data) {
DR16_t *dr16 = (DR16_t *)data;
return DR16_Init(dr16);
}
int8_t CMD_DR16_RC_GetInput(void *data, CMD_RawInput_t *output) {
DR16_t *dr16 = (DR16_t *)data;
memset(&output->rc, 0, sizeof(CMD_RawInput_RC_t));
output->online[CMD_SRC_RC] = dr16->header.online;
/* 遥控器摇杆映射 */
output->rc.joy_left.x = dr16->data.ch_l_x;
output->rc.joy_left.y = dr16->data.ch_l_y;
output->rc.joy_right.x = dr16->data.ch_r_x;
output->rc.joy_right.y = dr16->data.ch_r_y;
/* 拨杆映射 */
switch (dr16->data.sw_l) {
case DR16_SW_UP: output->rc.sw[0] = CMD_SW_UP; break;
case DR16_SW_MID: output->rc.sw[0] = CMD_SW_MID; break;
case DR16_SW_DOWN: output->rc.sw[0] = CMD_SW_DOWN; break;
default: output->rc.sw[0] = CMD_SW_ERR; break;
}
switch (dr16->data.sw_r) {
case DR16_SW_UP: output->rc.sw[1] = CMD_SW_UP; break;
case DR16_SW_MID: output->rc.sw[1] = CMD_SW_MID; break;
case DR16_SW_DOWN: output->rc.sw[1] = CMD_SW_DOWN; break;
default: output->rc.sw[1] = CMD_SW_ERR; break;
}
/* 拨轮映射 */
output->rc.dial = dr16->data.ch_res;
return CMD_OK;
}
int8_t CMD_DR16_PC_GetInput(void *data, CMD_RawInput_t *output) {
DR16_t *dr16 = (DR16_t *)data;
memset(&output->pc, 0, sizeof(CMD_RawInput_PC_t));
output->online[CMD_SRC_PC] = dr16->header.online;
/* PC端鼠标映射 */
output->pc.mouse.x = dr16->data.mouse.x;
output->pc.mouse.y = dr16->data.mouse.y;
output->pc.mouse.l_click = dr16->data.mouse.l_click;
output->pc.mouse.r_click = dr16->data.mouse.r_click;
/* 键盘映射 */
output->pc.keyboard.bitmap = dr16->raw_data.key;
return CMD_OK;
}
bool CMD_DR16_IsOnline(void *data) {
DR16_t *dr16 = (DR16_t *)data;
return dr16->header.online;
}
extern DR16_t cmd_dr16;
/* 定义适配器实例 */
CMD_DEFINE_ADAPTER(DR16_RC, cmd_dr16, CMD_SRC_RC, CMD_DR16_Init, CMD_DR16_RC_GetInput, CMD_DR16_IsOnline)
CMD_DEFINE_ADAPTER(DR16_PC, cmd_dr16, CMD_SRC_PC, CMD_DR16_Init, CMD_DR16_PC_GetInput, CMD_DR16_IsOnline)
#endif /* CMD_RC_DEVICE_TYPE == 0 */
/* ========================================================================== */
/* AT9S 适配器实现 (示例框架) */
/* ========================================================================== */
#if CMD_RC_DEVICE_TYPE == 1
int8_t CMD_AT9S_Init(void *data) {
AT9S_t *at9s = (AT9S_t *)data;
return AT9S_Init(at9s);
}
int8_t CMD_AT9S_GetInput(void *data, CMD_RawInput_t *output) {
AT9S_t *at9s = (AT9S_t *)data;
memset(output, 0, sizeof(CMD_RawInput_RC_t));
output->online[CMD_SRC_RC] = at9s->header.online;
/* TODO: 按照AT9S的数据格式进行映射 */
output->joy_left.x = at9s->data.ch_l_x;
output->joy_left.y = at9s->data.ch_l_y;
output->joy_right.x = at9s->data.ch_r_x;
output->joy_right.y = at9s->data.ch_r_y;
/* 拨杆映射需要根据AT9S的实际定义 */
return CMD_OK;
}
bool CMD_AT9S_IsOnline(void *data) {
AT9S_t *at9s = (AT9S_t *)data;
return at9s->header.online;
}
CMD_DEFINE_ADAPTER(AT9S, at9s, CMD_SRC_RC, CMD_AT9S_Init, CMD_AT9S_GetInput, CMD_AT9S_IsOnline)
#endif /* CMD_RC_DEVICE_TYPE == 1 */
/* ========================================================================== */
/* NUC/AI 适配器实现 */
/* ========================================================================== */
/* ========================================================================== */
/* REF/裁判系统 适配器实现 */
/* ========================================================================== */
#if CMD_ENABLE_SRC_REF
int8_t CMD_REF_AdapterInit(void *data) {
(void)data;
return CMD_OK;
}
int8_t CMD_REF_GetInput(void *data, CMD_RawInput_t *output) {
CMD_RawInput_REF_t *ref = (CMD_RawInput_REF_t *)data;
output->online[CMD_SRC_REF] = CMD_REF_IsOnline(ref);
output->ref = *ref;
return CMD_OK;
}
bool CMD_REF_IsOnline(void *data) {
CMD_RawInput_REF_t *ref = (CMD_RawInput_REF_t *)data;
return !(ref->chassis.ref_status == REF_STATUS_OFFLINE&&
ref->ai.ref_status == REF_STATUS_OFFLINE&&
ref->cap.ref_status == REF_STATUS_OFFLINE&&
ref->shoot.ref_status == REF_STATUS_OFFLINE);
}
CMD_DEFINE_ADAPTER(REF, cmd_ref, CMD_SRC_REF, CMD_REF_AdapterInit, CMD_REF_GetInput, CMD_REF_IsOnline)
#endif /* CMD_ENABLE_SRC_REF */
#if CMD_ENABLE_SRC_NUC
int8_t CMD_NUC_AdapterInit(void *data) {
/* NUC适配器不需要特殊初始化 */
return CMD_OK;
}
int8_t CMD_NUC_GetInput(void *data, CMD_RawInput_t *output) {
AI_cmd_t *ai_cmd = (AI_cmd_t *)data;
output->online[CMD_SRC_NUC] = true;
/* 映射AI数据到NUC输入结构 */
output->nuc.mode = ai_cmd->mode;
output->nuc.gimbal.setpoint.yaw = ai_cmd->gimbal.setpoint.yaw;
output->nuc.gimbal.setpoint.pit = ai_cmd->gimbal.setpoint.pit;
output->nuc.gimbal.accl.pit = ai_cmd->gimbal.accl.pit;
output->nuc.gimbal.accl.yaw = ai_cmd->gimbal.accl.yaw;
output->nuc.gimbal.vel.pit = ai_cmd->gimbal.vel.pit;
output->nuc.gimbal.vel.yaw = ai_cmd->gimbal.vel.yaw;
return CMD_OK;
}
bool CMD_NUC_IsOnline(void *data) {
return true;
}
/* 定义NUC适配器实例 */
extern AI_cmd_t ai_cmd;
CMD_DEFINE_ADAPTER(NUC, cmd_ai, CMD_SRC_NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput, CMD_NUC_IsOnline)
#endif /* CMD_ENABLE_SRC_NUC */
/* ========================================================================== */
/* 适配器管理实现 */
/* ========================================================================== */
int8_t CMD_Adapter_Register(CMD_InputAdapter_t *adapter) {
if (adapter == NULL || adapter->source >= CMD_SRC_NUM) {
return CMD_ERR_NULL;
}
g_adapters[adapter->source] = adapter;
return CMD_OK;
}
int8_t CMD_Adapter_InitAll(void) {
/* 注册编译时选择的RC设备适配器 */
#if CMD_RC_DEVICE_TYPE == 0
/* DR16 支持 RC 和 PC 输入 */
CMD_Adapter_Register(&g_adapter_DR16_RC);
CMD_Adapter_Register(&g_adapter_DR16_PC);
#elif CMD_RC_DEVICE_TYPE == 1
/* AT9S 目前只支持 RC 输入 */
CMD_Adapter_Register(&g_adapter_AT9S);
#endif
#if CMD_ENABLE_SRC_NUC
/* 注册NUC适配器 */
CMD_Adapter_Register(&g_adapter_NUC);
#endif
#if CMD_ENABLE_SRC_REF
/* 注册REF适配器 */
CMD_Adapter_Register(&g_adapter_REF);
#endif
/* 初始化所有已注册的适配器 */
for (int i = 0; i < CMD_SRC_NUM; i++) {
if (g_adapters[i] != NULL && g_adapters[i]->init != NULL) {
g_adapters[i]->init(g_adapters[i]->device_data);
}
}
return CMD_OK;
}
int8_t CMD_Adapter_GetInput(CMD_InputSource_t source, CMD_RawInput_t *output) {
if (source >= CMD_SRC_NUM || output == NULL) {
return CMD_ERR_NULL;
}
CMD_InputAdapter_t *adapter = g_adapters[source];
if (adapter == NULL || adapter->get_input == NULL) {
output->online[adapter->source] = false;
return CMD_ERR_NO_INPUT;
}
return adapter->get_input(adapter->device_data, output);
}
bool CMD_Adapter_IsOnline(CMD_InputSource_t source) {
if (source >= CMD_SRC_NUM) {
return false;
}
CMD_InputAdapter_t *adapter = g_adapters[source];
if (adapter == NULL || adapter->is_online == NULL) {
return false;
}
return adapter->is_online(adapter->device_data);
}

131
User/module/cmd/cmd_adapter.h Normal file
View File

@ -0,0 +1,131 @@
/*
* CMD 模块 V2 - 输入适配器接口
* 定义设备到统一输入结构的转换接口
*/
#pragma once
#include "cmd_types.h"
#ifdef __cplusplus
extern "C" {
#endif
/* ========================================================================== */
/* 适配器接口定义 */
/* ========================================================================== */
/* 适配器操作函数指针类型 */
typedef int8_t (*CMD_AdapterInitFunc)(void *device_data);
typedef int8_t (*CMD_AdapterGetInputFunc)(void *device_data, CMD_RawInput_t *output);
typedef bool (*CMD_AdapterIsOnlineFunc)(void *device_data);
/* 适配器描述结构 */
typedef struct {
const char *name; /* 适配器名称 */
CMD_InputSource_t source; /* 对应的输入源 */
void *device_data; /* 设备数据指针 */
CMD_AdapterInitFunc init; /* 初始化函数 */
CMD_AdapterGetInputFunc get_input; /* 获取输入函数 */
CMD_AdapterIsOnlineFunc is_online; /* 在线检测函数 */
} CMD_InputAdapter_t;
/* ========================================================================== */
/* 适配器注册宏 */
/* ========================================================================== */
/*
* 声明适配器
* 使用示例:
* CMD_DECLARE_ADAPTER(DR16, dr16, DR16_t)
*
* 会生成:
* - extern DR16_t dr16; // 设备实例声明
* - int8_t CMD_DR16_Init(void *data);
* - int8_t CMD_DR16_GetInput(void *data, CMD_RawInput_t *output);
* - bool CMD_DR16_IsOnline(void *data);
*/
#define CMD_DECLARE_ADAPTER(NAME, var, TYPE) \
int8_t CMD_##NAME##_Init(void *data); \
int8_t CMD_##NAME##_GetInput(void *data, CMD_RawInput_t *output); \
bool CMD_##NAME##_IsOnline(void *data);
/*
* 定义适配器实例
* 使用示例:
* CMD_DEFINE_ADAPTER(DR16_RC, dr16, CMD_SRC_RC, CMD_DR16_Init, CMD_DR16_RC_GetInput, CMD_DR16_RC_IsOnline)
*/
#define CMD_DEFINE_ADAPTER(NAME, var, source_enum, init_func, get_func, online_func) \
static CMD_InputAdapter_t g_adapter_##NAME = { \
.name = #NAME, \
.source = source_enum, \
.device_data = (void*)&var, \
.init = init_func, \
.get_input = get_func, \
.is_online = online_func, \
};
/* ========================================================================== */
/* RC设备适配器配置 */
/* ========================================================================== */
/* 选择使用的RC设备 - 只需修改这里 */
#define CMD_RC_DEVICE_TYPE 0 /* 0:DR16, 1:AT9S, 2:VT13 */
#if CMD_RC_DEVICE_TYPE == 0
#include "device/dr16.h"
CMD_DECLARE_ADAPTER(DR16_RC, dr16, DR16_t)
CMD_DECLARE_ADAPTER(DR16_PC, dr16, DR16_t)
#define CMD_RC_ADAPTER_NAME DR16
#define CMD_RC_ADAPTER_VAR dr16
#elif CMD_RC_DEVICE_TYPE == 1
#include "device/at9s_pro.h"
CMD_DECLARE_ADAPTER(AT9S, at9s, AT9S_t)
#define CMD_RC_ADAPTER_NAME AT9S
#define CMD_RC_ADAPTER_VAR at9s
#elif CMD_RC_DEVICE_TYPE == 2
#include "device/vt13.h"
CMD_DECLARE_ADAPTER(VT13, vt13, VT13_t)
#define CMD_RC_ADAPTER_NAME VT13
#define CMD_RC_ADAPTER_VAR vt13
#endif
/* ========================================================================== */
/* NUC/AI适配器配置 */
/* ========================================================================== */
#if CMD_ENABLE_SRC_NUC
#include "module/vision_bridge.h"
extern AI_cmd_t cmd_ai;
int8_t CMD_NUC_AdapterInit(void *data);
int8_t CMD_NUC_GetInput(void *data, CMD_RawInput_t *output);
bool CMD_NUC_IsOnline(void *data);
#endif
/* ========================================================================== */
/* REF/裁判系统适配器配置 */
/* ========================================================================== */
#if CMD_ENABLE_SRC_REF
extern CMD_RawInput_REF_t cmd_ref;
int8_t CMD_REF_AdapterInit(void *data);
int8_t CMD_REF_GetInput(void *data, CMD_RawInput_t *output);
bool CMD_REF_IsOnline(void *data);
#endif
/* ========================================================================== */
/* 适配器管理接口 */
/* ========================================================================== */
/* 初始化所有适配器 */
int8_t CMD_Adapter_InitAll(void);
/* 获取指定输入源的原始输入 */
int8_t CMD_Adapter_GetInput(CMD_InputSource_t source, CMD_RawInput_t *output);
/* 检查输入源是否在线 */
bool CMD_Adapter_IsOnline(CMD_InputSource_t source);
/* 注册适配器 (运行时注册，可选) */
int8_t CMD_Adapter_Register(CMD_InputAdapter_t *adapter);
#ifdef __cplusplus
}
#endif

223
User/module/cmd/cmd_behavior.c Normal file
View File

@ -0,0 +1,223 @@
/*
* CMD 模块 V2 - 行为处理器实现
*/
#include "cmd_behavior.h"
#include "cmd.h"
#if CMD_ENABLE_MODULE_GIMBAL
#include "module/gimbal.h"
#endif
#include <string.h>
/* ========================================================================== */
/* 行为回调函数 */
/* ========================================================================== */
/* 行为处理函数实现 */
int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_fast_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_slow_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_FIRE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.firecmd = true;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.mode = (ctx->output.shoot.cmd.mode + 1) % SHOOT_MODE_NUM;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR;
ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_AUTOAIM(CMD_t *ctx) {
/* 自瞄模式切换 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source == CMD_SRC_PC){
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
#if CMD_ENABLE_MODULE_REFUI
ctx->output.refui.source = CMD_SRC_NUC;
#endif
}
}
#endif
return CMD_OK;
}
int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
/* 切换RC和PC输入源 */
if (ctx->active_source == CMD_SRC_PC) {
ctx->active_source = CMD_SRC_RC;
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = CMD_SRC_RC;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = CMD_SRC_RC;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = CMD_SRC_RC;
#endif
#if CMD_ENABLE_MODULE_ARM
ctx->output.arm.source = CMD_SRC_RC;
#endif
} else if(ctx->active_source == CMD_SRC_RC) {
ctx->active_source = CMD_SRC_PC;
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_ARM
ctx->output.arm.source = CMD_SRC_PC;
#endif
}
return CMD_OK;
}
/* 行为配置表 - 由宏生成 */
static const CMD_BehaviorConfig_t g_behavior_configs[] = {
CMD_BEHAVIOR_TABLE(BUILD_BEHAVIOR_CONFIG)
};
/* ========================================================================== */
/* API实现 */
/* ========================================================================== */
int8_t CMD_Behavior_Init(void) {
/* 当前静态配置，无需初始化 */
return CMD_OK;
}
bool CMD_Behavior_IsTriggered(const CMD_RawInput_t *current,
const CMD_RawInput_t *last,
const CMD_BehaviorConfig_t *config) {
if (config == NULL || current == NULL) {
return false;
}
bool now_pressed = false;
bool last_pressed = false;
// 鼠标特殊按键处理
if (config->key == (CMD_KEY_L_CLICK)) {
now_pressed = current->pc.mouse.l_click;
last_pressed = last ? last->pc.mouse.l_click : false;
} else if (config->key == (CMD_KEY_R_CLICK)) {
now_pressed = current->pc.mouse.r_click;
last_pressed = last ? last->pc.mouse.r_click : false;
} else if (config->key == (CMD_KEY_M_CLICK)) {
now_pressed = current->pc.mouse.m_click;
last_pressed = last ? last->pc.mouse.m_click : false;
} else if (config->key == 0) {
return false;
} else {
// 多按键组合检测
now_pressed = ((current->pc.keyboard.bitmap & config->key) == config->key);
last_pressed = last ? ((last->pc.keyboard.bitmap & config->key) == config->key) : false;
}
switch (config->trigger) {
case CMD_ACTIVE_PRESSED:
return now_pressed;
case CMD_ACTIVE_RISING_EDGE:
return now_pressed && !last_pressed;
case CMD_ACTIVE_FALLING_EDGE:
return !now_pressed && last_pressed;
default:
return false;
}
}
int8_t CMD_Behavior_ProcessAll(CMD_t *ctx,
const CMD_RawInput_t *current,
const CMD_RawInput_t *last,
CMD_ModuleMask_t active_modules) {
if (ctx == NULL || current == NULL) {
return CMD_ERR_NULL;
}
for (size_t i = 0; i < BEHAVIOR_CONFIG_COUNT; i++) {
const CMD_BehaviorConfig_t *config = &g_behavior_configs[i];
/* 过滤模块掩码 */
if ((config->module_mask & active_modules) == 0) {
continue;
}
/* 检查是否触发 */
if (CMD_Behavior_IsTriggered(current, last, config)) {
if (config->handler != NULL) {
config->handler(ctx);
}
}
}
return CMD_OK;
}
const CMD_BehaviorConfig_t* CMD_Behavior_GetConfig(CMD_Behavior_t behavior) {
for (size_t i = 0; i < BEHAVIOR_CONFIG_COUNT; i++) {
if (g_behavior_configs[i].behavior == behavior) {
return &g_behavior_configs[i];
}
}
return NULL;
}

69
User/module/cmd/cmd_behavior.h Normal file
View File

@ -0,0 +1,69 @@
/*
* CMD 模块 V2 - 行为处理器
* 实现PC端按键到行为的映射和处理
*/
#pragma once
#include "cmd_types.h"
#ifdef __cplusplus
extern "C" {
#endif
/* ========================================================================== */
/* 行为处理器接口 */
/* ========================================================================== */
/* 行为处理函数类型 */
struct CMD_Context; /* 前向声明 */
typedef int8_t (*CMD_BehaviorHandler)(struct CMD_Context *ctx);
/* 行为配置项 */
typedef struct {
CMD_Behavior_t behavior; /* 行为枚举 */
uint32_t key; /* 绑定的按键 */
CMD_TriggerType_t trigger; /* 触发类型 */
CMD_ModuleMask_t module_mask; /* 影响的模块 */
CMD_BehaviorHandler handler; /* 处理函数 */
} CMD_BehaviorConfig_t;
/* ========================================================================== */
/* 行为表生成宏 */
/* ========================================================================== */
/* 从宏表生成配置数组 */
#define BUILD_BEHAVIOR_CONFIG(name, key, trigger, mask) \
{ CMD_BEHAVIOR_##name, key, trigger, mask, CMD_Behavior_Handle_##name },
/* 声明所有行为处理函数 */
#define DECLARE_BEHAVIOR_HANDLER(name, key, trigger, mask) \
int8_t CMD_Behavior_Handle_##name(struct CMD_Context *ctx);
/* 展开声明 */
CMD_BEHAVIOR_TABLE(DECLARE_BEHAVIOR_HANDLER)
#undef DECLARE_BEHAVIOR_HANDLER
/* ========================================================================== */
/* 行为处理器API */
/* ========================================================================== */
/* 初始化行为处理器 */
int8_t CMD_Behavior_Init(void);
/* 检查行为是否被触发 */
bool CMD_Behavior_IsTriggered(const CMD_RawInput_t *current,
const CMD_RawInput_t *last,
const CMD_BehaviorConfig_t *config);
/* 处理所有触发的行为 */
int8_t CMD_Behavior_ProcessAll(struct CMD_Context *ctx,
const CMD_RawInput_t *current,
const CMD_RawInput_t *last,
CMD_ModuleMask_t active_modules);
/* 获取行为配置 */
const CMD_BehaviorConfig_t* CMD_Behavior_GetConfig(CMD_Behavior_t behavior);
#ifdef __cplusplus
}
#endif

167
User/module/cmd/cmd_example.c Normal file
View File

@ -0,0 +1,167 @@
/*
* CMD 模块 V2 - 使用示例和配置模板
*
* 本文件展示如何配置和使用新的CMD模块
*/
#include "cmd.h"
/* ========================================================================== */
/* config示例 */
/* ========================================================================== */
/* 默认配置 */
// static CMD_Config_t g_cmd_config = {
// /* 灵敏度设置 */
// .sensitivity = {
// .mouse_sens = 0.8f,
// .move_sens = 1.0f,
// .move_fast_mult = 1.5f,
// .move_slow_mult = 0.5f,
// },
// /* RC拨杆模式映射 */
// .rc_mode_map = {
// /* 左拨杆控制底盘模式 */
// .sw_left_up = CHASSIS_MODE_BREAK,
// .sw_left_mid = CHASSIS_MODE_FOLLOW_GIMBAL,
// .sw_left_down = CHASSIS_MODE_ROTOR,
// /* 用于云台模式 */
// .gimbal_sw_up = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE,
// .gimbal_sw_down = GIMBAL_MODE_RELATIVE,
// },
// };
// /* CMD上下文 */
// static CMD_t g_cmd_ctx;
/* ========================================================================== */
/* 队列创建示例 */
/* ========================================================================== */
// #if CMD_RCTypeTable_Index == 0
// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
// #elif CMD_RCTypeTable_Index == 1
// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL);
// #endif
/* ========================================================================== */
/* 任务示例 */
/* ========================================================================== */
// #if CMD_RCTypeTable_Index == 0
// DR16_t cmd_dr16;
// #elif CMD_RCTypeTable_Index == 1
// AT9S_t cmd_at9s;
// #endif
// Shoot_CMD_t *cmd_for_shoot;
// Chassis_CMD_t *cmd_for_chassis;
// Gimbal_CMD_t *cmd_for_gimbal;
// static CMD_t cmd;
// void Task_cmd() {
// CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param);
// while (1) {
// #if CMD_RCTypeTable_Index == 0
// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_dr16, NULL, 0);
// #elif CMD_RCTypeTable_Index == 1
// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0);
// #endif
// CMD_Update(&cmd);
// /* 获取命令发送到各模块 */
// cmd_for_chassis = CMD_GetChassisCmd(&cmd);
// cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
// cmd_for_shoot = CMD_GetShootCmd(&cmd);
// osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0);
// osMessageQueueReset(task_runtime.msgq.shoot.cmd);
// osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0);
// osMessageQueueReset(task_runtime.msgq.chassis.cmd);
// osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
// }
// }
/* ========================================================================== */
/* 架构说明 */
/* ========================================================================== */
/*
* ## 新架构优势
*
* ### 1. 统一的输入抽象层 (CMD_RawInput_t)
* - 所有设备（DR16/AT9S/VT13等）都转换成相同格式
* - 上层代码无需关心具体设备类型
* - 添加新设备只需实现适配器，不改动主逻辑
*
* ### 2. 适配器模式
* - 每个设备一个适配器文件
* - 实现 Init, GetInput, IsOnline 三个函数
* - 通过宏选择编译哪个适配器
*
* ### 3. X-Macro配置表
* - CMD_INPUT_SOURCE_TABLE: 配置输入源
* - CMD_OUTPUT_MODULE_TABLE: 配置输出模块
* - CMD_BEHAVIOR_TABLE: 配置按键行为映射
* - 编译时生成枚举、配置数组、处理函数
*
* ### 4. 行为驱动设计
* - 行为与按键解耦
* - 运行时可修改映射
* - 支持边沿触发和持续触发
*
* ### 5. 清晰的分层
*
* ┌──────────────────────────────────────┐
* │ 应用层 (cmd.c) │
* │ - CMD_Update() │
* │ - 仲裁、命令生成 │
* └──────────────┬───────────────────────┘
* │
* ┌──────────────▼───────────────────────┐
* │ 行为处理层 (cmd_behavior.c) │
* │ - 按键触发检测 │
* │ - 行为函数调用 │
* └──────────────┬───────────────────────┘
* │
* ┌──────────────▼──────────────────────────┐
* │ 抽象输入层 (cmd_types.h) │
* │ - 多输入源操作同一CMD_RawInput_t不同分区 │
* │ - 统一的摇杆、开关、键鼠结构 │
* └──────────────┬──────────────────────────┘
* │
* ┌──────────────▼───────────────────────┐
* │ 适配器层 (cmd_adapter.c) │
* │ - DR16_Adapter │
* │ - AT9S_Adapter │
* │ - 设备数据 → CMD_RawInput_t │
* └──────────────────────────────────────┘
*
* ## 扩展指南
*
* ### 添加新遥控器设备
* 1. 在 cmd_adapter.h 中添加宏定义选项
* 2. 在 cmd_adapter.c 中实现三个适配器函数
* 3. 修改 CMD_RC_DEVICE_TYPE 宏选择新设备
*
* ### 添加新输入源（如自定义协议）
* 1. 在 CMD_INPUT_SOURCE_TABLE 添加条目
* 2. 实现对应的适配器
* 3. 在 CMD_GenerateCommands 添加处理分支
*
* ### 添加新行为
* 1. 在 CMD_BEHAVIOR_TABLE 添加条目,并修正BEHAVIOR_CONFIG_COUNT
* 2. 实现 CMD_Behavior_Handle_XXX 函数
*
* ### 添加新输出模块
* 1. 在 CMD_OUTPUT_MODULE_TABLE 添加条目
* 2. 在 CMD_t 中添加输出成员
* 3. 实现对应的 BuildXXXCmd 函数
*/

66
User/module/cmd/cmd_feature.h Normal file
View File

@ -0,0 +1,66 @@
/*
* CMD 模块 V2 - 功能特性开关配置
*
* 修改此文件来快速使能/失能各个模块和输入源。
* 失能后，对应的代码和头文件依赖将被完全排除在编译之外。
*/
#pragma once
/* ========================================================================== */
/* 输入源使能开关 */
/* ========================================================================== */
/** 遥控器输入 (DR16 / AT9S 等) */
#define CMD_ENABLE_SRC_RC 1
/** PC 端键鼠输入 (通过 DR16 转发) */
#define CMD_ENABLE_SRC_PC 1
/** NUC / AI 输入 (需要 vision_bridge 模块) */
#define CMD_ENABLE_SRC_NUC 0
/**
* 裁判系统数据中转开关
* 1 (比赛模式): cmd 将 referee 数据转发到各模块的 .ref 队列
* 0 (调试模式): cmd 不转发，裁判系统可断开，不影响其他功能
*/
#define CMD_ENABLE_SRC_REF 1
/* ========================================================================== */
/* 输出模块使能开关 */
/* ========================================================================== */
/** 底盘模块 (需要 module/chassis.h) */
#define CMD_ENABLE_MODULE_CHASSIS 0
/** 云台模块 (需要 module/gimbal.h) */
#define CMD_ENABLE_MODULE_GIMBAL 1
/** 射击模块 (需要 module/shoot.h) */
#define CMD_ENABLE_MODULE_SHOOT 1
/** 履带模块 (需要 module/track.h) */
#define CMD_ENABLE_MODULE_TRACK 0
/** 机械臂模块 (需要 component/arm_kinematics/arm6dof.h) */
#define CMD_ENABLE_MODULE_ARM 0
/** 裁判系统UI命令模块 (需要 device/referee.h) */
#define CMD_ENABLE_MODULE_REFUI 0
/** 平衡底盘模块 (需要 module/balance_chassis.h) */
#define CMD_ENABLE_MODULE_BALANCE_CHASSIS 1
/* ========================================================================== */
/* 合法性检查 */
/* ========================================================================== */
/* PC输入源依赖RC适配器共同存在（DR16同时提供RC和PC数据） */
#if CMD_ENABLE_SRC_PC && !CMD_ENABLE_SRC_RC
#error "CMD_ENABLE_SRC_PC requires CMD_ENABLE_SRC_RC (both share the DR16 adapter)"
#endif
/* NUC依赖vision_bridge模块，确保已包含相关模块 */
/* #if CMD_ENABLE_SRC_NUC && !defined(VISION_BRIDGE_ENABLED)
#error "CMD_ENABLE_SRC_NUC requires vision_bridge module"
#endif */

299
User/module/cmd/cmd_types.h Normal file
View File

@ -0,0 +1,299 @@
/*
* CMD 模块 V2 - 类型定义
* 统一的输入/输出抽象层
*/
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "cmd_feature.h" /* 功能特性开关 */
#ifdef __cplusplus
extern "C" {
#endif
/* ========================================================================== */
/* 错误码定义 */
/* ========================================================================== */
#define CMD_OK (0)
#define CMD_ERR_NULL (-1)
#define CMD_ERR_MODE (-2)
#define CMD_ERR_SOURCE (-3)
#define CMD_ERR_NO_INPUT (-4)
/* ========================================================================== */
/* 输入源配置宏表 */
/* ========================================================================== */
/*
* 使用方法：在 cmd_feature.h 中设置各 CMD_ENABLE_SRC_xxx 宏。
* 格式: X(枚举名, 适配器初始化函数, 获取数据函数)
*/
/* 各输入源条件展开辅助宏 */
#if CMD_ENABLE_SRC_RC
#define _X_SRC_RC(X) X(RC, CMD_RC_AdapterInit, CMD_RC_GetInput)
#else
#define _X_SRC_RC(X)
#endif
#if CMD_ENABLE_SRC_PC
#define _X_SRC_PC(X) X(PC, CMD_PC_AdapterInit, CMD_PC_GetInput)
#else
#define _X_SRC_PC(X)
#endif
#if CMD_ENABLE_SRC_NUC
#define _X_SRC_NUC(X) X(NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput)
#else
#define _X_SRC_NUC(X)
#endif
#if CMD_ENABLE_SRC_REF
#define _X_SRC_REF(X) X(REF, CMD_REF_AdapterInit, CMD_REF_GetInput)
#else
#define _X_SRC_REF(X)
#endif
#define CMD_INPUT_SOURCE_TABLE(X) \
_X_SRC_RC(X) \
_X_SRC_PC(X) \
_X_SRC_NUC(X) \
_X_SRC_REF(X)
/* 各输出模块条件展开辅助宏 */
#if CMD_ENABLE_MODULE_CHASSIS
#define _X_MOD_CHASSIS(X) X(CHASSIS, Chassis_CMD_t, chassis)
#else
#define _X_MOD_CHASSIS(X)
#endif
#if CMD_ENABLE_MODULE_GIMBAL
#define _X_MOD_GIMBAL(X) X(GIMBAL, Gimbal_CMD_t, gimbal)
#else
#define _X_MOD_GIMBAL(X)
#endif
#if CMD_ENABLE_MODULE_SHOOT
#define _X_MOD_SHOOT(X) X(SHOOT, Shoot_CMD_t, shoot)
#else
#define _X_MOD_SHOOT(X)
#endif
#if CMD_ENABLE_MODULE_TRACK
#define _X_MOD_TRACK(X) X(TRACK, Track_CMD_t, track)
#else
#define _X_MOD_TRACK(X)
#endif
#if CMD_ENABLE_MODULE_ARM
#define _X_MOD_ARM(X) X(ARM, Arm_CMD_t, arm)
#else
#define _X_MOD_ARM(X)
#endif
#if CMD_ENABLE_MODULE_REFUI
#define _X_MOD_REFUI(X) X(REFUI, Referee_UI_CMD_t, refui)
#else
#define _X_MOD_REFUI(X)
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
#define _X_MOD_BALANCE_CHASSIS(X) X(BALANCE_CHASSIS, Chassis_CMD_t, balance_chassis)
#else
#define _X_MOD_BALANCE_CHASSIS(X)
#endif
/* 输出模块配置宏表 */
#define CMD_OUTPUT_MODULE_TABLE(X) \
_X_MOD_CHASSIS(X) \
_X_MOD_GIMBAL(X) \
_X_MOD_SHOOT(X) \
_X_MOD_TRACK(X) \
_X_MOD_ARM(X) \
_X_MOD_REFUI(X) \
_X_MOD_BALANCE_CHASSIS(X) \
/* ========================================================================== */
/* 输入源枚举 */
/* ========================================================================== */
#define ENUM_INPUT_SOURCE(name, ...) CMD_SRC_##name,
typedef enum {
CMD_INPUT_SOURCE_TABLE(ENUM_INPUT_SOURCE)
CMD_SRC_NUM
} CMD_InputSource_t;
#undef ENUM_INPUT_SOURCE
/* ========================================================================== */
/* 统一输入数据结构 */
/* ========================================================================== */
/* 摇杆数据 - 统一为-1.0 ~ 1.0 */
typedef struct {
float x;
float y;
} CMD_Joystick_t;
/* 开关位置 */
typedef enum {
CMD_SW_ERR = 0,
CMD_SW_UP,
CMD_SW_MID,
CMD_SW_DOWN,
} CMD_SwitchPos_t;
/* 鼠标数据 */
typedef struct {
int16_t x; /* 鼠标X轴移动速度 */
int16_t y; /* 鼠标Y轴移动速度 */
int16_t z; /* 鼠标滚轮 */
bool l_click; /* 左键 */
bool r_click; /* 右键 */
bool m_click; /* 中键 */
} CMD_Mouse_t;
/* 键盘数据 - 最多支持32个按键 */
typedef struct {
uint32_t bitmap; /* 按键位图 */
} CMD_Keyboard_t;
/* 键盘按键索引 */
typedef enum {
CMD_KEY_W = (1 << 0), CMD_KEY_S = (1 << 1), CMD_KEY_A = (1 << 2), CMD_KEY_D = (1 << 3),
CMD_KEY_SHIFT = (1 << 4), CMD_KEY_CTRL = (1 << 5), CMD_KEY_Q = (1 << 6), CMD_KEY_E = (1 << 7),
CMD_KEY_R = (1 << 8), CMD_KEY_F = (1 << 9), CMD_KEY_G = (1 << 10), CMD_KEY_Z = (1 << 11),
CMD_KEY_X = (1 << 12), CMD_KEY_C = (1 << 13), CMD_KEY_V = (1 << 14), CMD_KEY_B = (1 << 15),
CMD_KEY_NUM
} CMD_KeyIndex_t;
typedef struct {
CMD_Joystick_t joy_left; /* 左摇杆 */
CMD_Joystick_t joy_right; /* 右摇杆 */
CMD_SwitchPos_t sw[4]; /* 4个拨杆 */
float dial; /* 拨轮 */
} CMD_RawInput_RC_t;
typedef struct {
CMD_Mouse_t mouse;
CMD_Keyboard_t keyboard;
} CMD_RawInput_PC_t;
/* AI输入数据 */
typedef struct {
uint8_t mode;
struct {
struct {
float yaw;
float pit;
} setpoint;
struct {
float pit;
float yaw;
} accl;
struct {
float pit;
float yaw;
} vel;
} gimbal;
} CMD_RawInput_NUC_t;
#if CMD_ENABLE_SRC_REF
#include "device/referee_proto_types.h"
/* 裁判系统原始输入，包含需转发给各模块的完整子集 */
typedef struct {
Referee_ForChassis_t chassis;
Referee_ForShoot_t shoot;
Referee_ForCap_t cap;
Referee_ForAI_t ai;
} CMD_RawInput_REF_t;
#endif
/* 统一的原始输入结构 - 所有设备适配后都转换成这个格式 */
typedef struct {
bool online[CMD_SRC_NUM];
#if CMD_ENABLE_SRC_RC
/* 遥控器部分 */
CMD_RawInput_RC_t rc;
#endif
#if CMD_ENABLE_SRC_PC
/* PC部分 */
CMD_RawInput_PC_t pc;
#endif
#if CMD_ENABLE_SRC_NUC
/* NUC部分 */
CMD_RawInput_NUC_t nuc;
#endif
#if CMD_ENABLE_SRC_REF
/* REF部分 - 裁判系统数据 */
CMD_RawInput_REF_t ref;
#endif
} CMD_RawInput_t;
/* ========================================================================== */
/* 模块掩码 */
/* ========================================================================== */
typedef enum {
CMD_MODULE_NONE = (1 << 0),
CMD_MODULE_CHASSIS = (1 << 1),
CMD_MODULE_GIMBAL = (1 << 2),
CMD_MODULE_SHOOT = (1 << 3),
CMD_MODULE_TRACK = (1 << 4),
CMD_MODULE_ARM = (1 << 5),
CMD_MODULE_REFUI = (1 << 6),
CMD_MODULE_BALANCE_CHASSIS = (1 << 7),
CMD_MODULE_ALL = 0xFE
} CMD_ModuleMask_t;
/* ========================================================================== */
/* 行为定义 */
/* ========================================================================== */
/* 行为-按键映射宏表 */
#define BEHAVIOR_CONFIG_COUNT (11)
#define CMD_BEHAVIOR_TABLE(X) \
X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(DECELERATE, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(FIRE, CMD_KEY_L_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_SHOOT) \
X(FIRE_MODE, CMD_KEY_B, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_SHOOT) \
X(ROTOR, CMD_KEY_E, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \
X(AUTOAIM, CMD_KEY_R_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_NONE) \
X(CHECKSOURCERCPC, CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_V, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE)
/* 触发类型 */
typedef enum {
CMD_ACTIVE_PRESSED, /* 按住时触发 */
CMD_ACTIVE_RISING_EDGE, /* 按下瞬间触发 */
CMD_ACTIVE_FALLING_EDGE, /* 松开瞬间触发 */
} CMD_TriggerType_t;
/* 特殊按键值 */
#define CMD_KEY_NONE 0xFF
#define CMD_KEY_L_CLICK (1 << 31)
#define CMD_KEY_R_CLICK (1 << 30)
#define CMD_KEY_M_CLICK (1 << 29)
/* 行为枚举 - 由宏表自动生成 */
#define ENUM_BEHAVIOR(name, key, trigger, mask) CMD_BEHAVIOR_##name,
typedef enum {
CMD_BEHAVIOR_TABLE(ENUM_BEHAVIOR)
CMD_BEHAVIOR_NUM
} CMD_Behavior_t;
#undef ENUM_BEHAVIOR
/* ========================================================================== */
/* 键盘辅助宏 */
/* ========================================================================== */
#define CMD_KEY_PRESSED(kb, key) (((kb)->bitmap >> (key)) & 1)
#define CMD_KEY_SET(kb, key) ((kb)->bitmap |= (1 << (key)))
#define CMD_KEY_CLEAR(kb, key) ((kb)->bitmap &= ~(1 << (key)))
#ifdef __cplusplus
}
#endif

141
User/module/config.c
View File

@ -20,7 +20,10 @@
*/
Config_RobotParam_t robot_config = {
/* USER CODE BEGIN robot_config */
.ref_screen={
.width=1920,
.height=1080,
},
.gimbal_param = {
.pid = {
.yaw_omega = {
@ -103,30 +106,59 @@ Config_RobotParam_t robot_config = {
.shoot_param = {
.trig_step_angle=M_2PI/8,
.shot_delay_time=0.05f,
.shot_burst_num=1,
.fric_motor_param[0] = {
.basic={
.projectileType=SHOOT_PROJECTILE_17MM,
.fric_num=2,
.extra_deceleration_ratio=1.0f,
.num_trig_tooth=8,
.shot_freq=1.0f,
.shot_burst_num=3,
.ratio_multilevel = {1.0f},
},
.jamDetection={
.enable=true,
.threshold=200.0f,
.suspectedTime=0.5f,
},
.heatControl={
.enable=true,
.nmax=18.0f, // 最大射频 Hz
.Hwarn=200.0f, // 热量预警值
.Hsatu=100.0f, // 热量饱和值
.Hthres=50.0f, // 热量阈值
},
.motor={
.fric = {
{
.param = {
.can = BSP_CAN_1,
.id = 0x201,
.module = MOTOR_M3508,
.reverse = false,
.gear=false,
},
.fric_motor_param[1] = {
.level=1,
},
{
.param = {
.can = BSP_CAN_1,
.id = 0x202,
.module = MOTOR_M3508,
.reverse = true,
.gear=false,
},
.trig_motor_param = {
.level=1,
}
},
.trig = {
.can = BSP_CAN_1,
.id = 0x203,
.module = MOTOR_M2006,
.reverse = true,
.gear=true,
},
},
.pid={
.fric_follow = {
.k=1.0f,
.p=1.5f,
@ -137,28 +169,17 @@ Config_RobotParam_t robot_config = {
.d_cutoff_freq=30.0f,
.range=-1.0f,
},
.fric_err = {
.k=1.0f,
.p=4.0f,
.i=0.4f,
.d=0.04f,
.i_limit=0.25f,
.out_limit=0.25f,
.d_cutoff_freq=40.0f,
.k=0.0f,
.p=0.0f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=0.0f,
.d_cutoff_freq=0.0f,
.range=-1.0f,
},
.trig_omg = {
.k=1.0f,
.p=1.5f,
.i=0.3f,
.d=0.5f,
.i_limit=0.2f,
.out_limit=0.9f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
},
.trig = {
.trig_2006 = {
.k=1.0f,
.p=1.0f,
.i=0.1f,
@ -168,16 +189,48 @@ Config_RobotParam_t robot_config = {
.d_cutoff_freq=-1.0f,
.range=M_2PI,
},
.filter.fric = {
.trig_omg_2006 = {
.k=1.0f,
.p=1.5f,
.i=0.3f,
.d=0.5f,
.i_limit=0.2f,
.out_limit=0.9f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
},
.trig_3508 = {
.k=0.5f,
.p=1.8f,
.i=0.3f,
.d=0.1f,
.i_limit=0.15f,
.out_limit=1.0f,
.d_cutoff_freq=-1.0f,
.range=M_2PI,
},
.trig_omg_3508 = {
.k=1.0f,
.p=1.0f,
.i=0.0f,
.d=0.0f,
.i_limit=0.0f,
.out_limit=1.0f,
.d_cutoff_freq=-1.0f,
.range=-1.0f,
},
},
.filter={
.fric = {
.in = 30.0f,
.out = 30.0f,
},
.filter.trig = {
.trig = {
.in = 30.0f,
.out = 30.0f,
},
},
},
.chassis_param = {
.yaw={
.k=1.0f,
@ -385,6 +438,36 @@ Config_RobotParam_t robot_config = {
.can = BSP_FDCAN_2,
.vision_id = 0x104,
},
.cmd_param = {
.source_priority = {
#if CMD_ENABLE_SRC_RC
[0] = CMD_SRC_RC,
#endif
#if CMD_ENABLE_SRC_PC
[1] = CMD_SRC_PC,
#endif
},
.sensitivity = {
.mouse_sens = 60.0f,
.move_sens = 1.0f,
.move_fast_mult = 2.0f,
.move_slow_mult = 0.4f,
},
.rc_mode_map = {
#if CMD_ENABLE_MODULE_GIMBAL
.gimbal_sw_up = GIMBAL_MODE_RELAX,
.gimbal_sw_mid = GIMBAL_MODE_RELATIVE,
.gimbal_sw_down = GIMBAL_MODE_RELATIVE,
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
.balance_sw_up = CHASSIS_MODE_RELAX,
.balance_sw_mid = CHASSIS_MODE_RELAX,
.balance_sw_down = CHASSIS_MODE_WHELL_LEG_BALANCE,
#endif
},
},
/* USER CODE END robot_config */
};

6
User/module/config.h
View File

@ -13,7 +13,9 @@ extern "C" {
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "device/vision_bridge.h"
#include "module/vision_bridge.h"
#include "module/cmd/cmd.h"
#include "device/referee_proto_types.h"
/**
* @brief 机器人参数配置结构体
* @note 在此添加您的配置参数
@ -24,6 +26,8 @@ typedef struct {
Chassis_Params_t chassis_param;
Gimbal_Params_t gimbal_param;
AI_Param_t ai_param;
CMD_Config_t cmd_param;
Referee_Screen_t ref_screen;
/* USER CODE END Config_RobotParam */
} Config_RobotParam_t;

9
User/module/gimbal.c
View File

@ -258,3 +258,12 @@ void Gimbal_Output(Gimbal_t *g){
MOTOR_RM_Ctrl(&g->param->pit_motor);
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
}
/**
* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Gimbal_RefereeUI_t *ui) {
ui->mode = g->mode;
}

5
User/module/gimbal.h
View File

@ -33,6 +33,11 @@ typedef enum {
GIMBAL_MODE_AI_CONTROL /* AI控制模式，直接接受AI下发的目标角度 */
} Gimbal_Mode_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
Gimbal_Mode_t mode;
} Gimbal_RefereeUI_t;
typedef struct {
Gimbal_Mode_t mode;
float delta_yaw;

985
User/module/shoot.c
View File

File diff suppressed because it is too large Load Diff

268
User/module/shoot.h
View File

@ -1,156 +1,243 @@
/*
* far蛇模组
* far♂蛇模块
*/
#pragma once
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h"
#include <stdbool.h>
#include "component/pid.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define SHOOT_OK (0) /* 运行正常 */
#define SHOOT_ERR (-1) /* 运行时发现了其他错误 */
#define SHOOT_ERR_NULL (-2) /* 运行时发现NULL指针 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的CMD_ChassisMode_t */
#define SHOOT_ERR_TYPE (-4) /* 运行时配置了错误的Chassis_Type_t */
#define MAX_FRIC_NUM 2
#define MAX_NUM_MULTILEVEL 1 /* 多级发射级数 */
#define SHOOT_FRIC_NUM (2) /* 摩擦轮数量 */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 5000.0f
#define SHOOT_OK (0) /* 运行正常 */
#define SHOOT_ERR_NULL (-1) /* 运行时发现NULL指针 */
#define SHOOT_ERR_ERR (-2) /* 运行时发现了其他错误 */
#define SHOOT_ERR_MODE (-3) /* 运行时配置了错误的Mode */
#define SHOOT_ERR_MOTOR (-4) /* 运行时配置了不存在的电机类型 */
#define SHOOT_ERR_MALLOC (-5) /* 内存分配失败 */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
SHOOT_JAMFSM_STATE_NORMAL = 0,/* 常规状态 */
SHOOT_JAMFSM_STATE_SUSPECTED, /* 怀疑状态 */
SHOOT_JAMFSM_STATE_CONFIRMED, /* 确认状态 */
SHOOT_JAMFSM_STATE_DEAL /* 处理状态 */
}Shoot_JamDetectionFSM_State_t;
typedef enum {
SHOOT_STATE_IDLE = 0, // 熄火
SHOOT_STATE_READY, // 准备射击
SHOOT_STATE_FIRE // 射击
} Shoot_State_t;
SHOOT_HEAT_DETECT_IDLE = 0, /* 停机状态 */
SHOOT_HEAT_DETECT_READY, /* 准备检测状态 */
SHOOT_HEAT_DETECT_SUSPECTED, /* 发射嫌疑状态 */
SHOOT_HEAT_DETECT_CONFIRMED, /* 确认发射状态 */
SHOOT_HEAT_DETECT_RECOVERING /* 发射恢复状态 */
}Shoot_HeatDetectionFSM_State_t;
typedef enum {
SHOOT_STATE_IDLE = 0,/* 熄火 */
SHOOT_STATE_READY, /* 准备射击 */
SHOOT_STATE_FIRE /* 射击 */
}Shoot_Running_State_t;
typedef enum {
SHOOT_MODE_SAFE = 0, // 安全模式
SHOOT_MODE_SINGLE, // 单发模式
SHOOT_MODE_BURST, // 多发模式
SHOOT_MODE_CONTINUE // 连发模式
} Shoot_Mode_t;
SHOOT_MODE_SAFE = 0,/* 安全模式 */
SHOOT_MODE_SINGLE, /* 单发模式 */
SHOOT_MODE_BURST, /* 多发模式 */
SHOOT_MODE_CONTINUE,/* 连发模式 */
SHOOT_MODE_NUM
}Shoot_Mode_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
bool online;
Shoot_Mode_t mode;
Shoot_Running_State_t fire;
} Shoot_RefereeUI_t;
bool ready; /* 准备射击 */
bool firecmd; /* 射击指令 */
typedef enum {
SHOOT_PROJECTILE_17MM,
SHOOT_PROJECTILE_42MM,
}Shoot_Projectile_t;
typedef struct{
MOTOR_RM_Param_t param;
uint8_t level; /* 电机属于第几级发射；1起始 */
}Shoot_MOTOR_RM_Param_t;
} Shoot_CMD_t;
typedef struct {
MOTOR_Feedback_t fric[SHOOT_FRIC_NUM]; /* 摩擦轮电机反馈 */
MOTOR_Feedback_t trig; /* 拨弹电机反馈 */
float fil_fric_rpm[SHOOT_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
float trig_angle_cicle; /* 拨弹电机减速输出轴单圈角度（0~M_2PI） */
float fric_rpm[SHOOT_FRIC_NUM]; /* 归一化摩擦轮转速 */
float fric_avgrpm; /* 归一化摩擦轮平均转速*/
float trig_rpm; /* 归一化拨弹电机转速*/
MOTOR_Feedback_t fric[MAX_FRIC_NUM];/* 摩擦轮电机反馈 */
MOTOR_RM_t trig; /* 拨弹电机反馈 */
}Shoot_Feedback_t;
typedef struct{
float time_last_shoot;
uint8_t num_to_shoot;
uint8_t num_shooted;
}Shoot_AngleCalu_t;
float fil_rpm[MAX_FRIC_NUM]; /* 滤波后的摩擦轮原始转速 */
float normalized_fil_rpm[MAX_FRIC_NUM]; /* 归一化摩擦轮转速 */
float normalized_fil_avgrpm[MAX_NUM_MULTILEVEL]; /* 归一化摩擦轮平均转速 */
float coupled_control_weights; /* 耦合控制权重 */
}Shoot_VARSForFricCtrl_t;
typedef struct{
float time_lastShoot;/* 上次射击时间 */
uint16_t num_toShoot;/* 剩余待发射弹数 */
uint16_t num_shooted;/* 已发射弹数 */
float trig_agl; /*计算所有减速比后的拨弹盘的角度*/
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
float trig_rpm; /* 归一化拨弹电机转速 */
}Shoot_VARSForTrigCtrl_t;
typedef struct {
float out_follow[SHOOT_FRIC_NUM];
float out_err[SHOOT_FRIC_NUM];
float out_fric[SHOOT_FRIC_NUM];
float lpfout_fric[SHOOT_FRIC_NUM];
bool detected; /* 卡弹检测结果 */
float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */
Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */
}Shoot_JamDetection_t;
typedef struct {
/* 从裁判系统读取的常量 */
float Hmax; // 热量上限
float Hcd; // 热量冷却速度
float Hgen; // 每发射一发产生的热量
/* 实时数据 */
float Hnow; // 当前热量（从裁判系统实时读取）
float Hnow_last; // 上次裁判系统热量值（用于检测更新）
float Hres; // 剩余热量 (Hmax - Hnow)
/* 控制变量 */
float ncd; // 冷却射频（消耗热量 = 自然恢复热量）
/* 发射检测状态机 */
Shoot_HeatDetectionFSM_State_t detectState; // 检测状态
float detectTime; // 检测计时器
float avgFricRpm; // 摩擦轮平均转速
float baselineRpm; // 动态基准转速，用于连发掉速检测的参照
float valleyRpm; // 掉速谷底转速，用于判断掉速回升
float rpmDrop; // 转速下降量
bool shotDetected; // 检测到发射标志
/* 自主热量估计 */
float Hnow_estimated; // 估计的当前热量
float Hnow_fused; // 融合后的热量值
uint16_t shots_detected; // 检测到的发射数
uint16_t shots_available;// 当前热量可发射弹数
uint16_t unverified_shots; // 已经拨弹但等待摩擦轮掉速确认的弹丸数
}Shoot_HeatControl_t;
typedef struct {
float out_follow[MAX_FRIC_NUM];
float out_err[MAX_FRIC_NUM];
float out_fric[MAX_FRIC_NUM];
float lpfout_fric[MAX_FRIC_NUM];
float outagl_trig;
float outomg_trig;
float outlpf_trig;
}Shoot_Output_t;
typedef struct {
Shoot_Mode_t mode;/* 射击模式 */
bool ready; /* 准备射击 */
bool firecmd; /* 射击 */
}Shoot_CMD_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
float trig_step_angle; /* 每发弹丸拨弹电机转动的角度 */
float shot_delay_time; /* 射击间隔时间，单位秒 */
uint8_t shot_burst_num; /* 多发模式下一次射击的发数 */
MOTOR_RM_Param_t fric_motor_param[SHOOT_FRIC_NUM];
MOTOR_RM_Param_t trig_motor_param;
struct{
Shoot_Projectile_t projectileType; /* 发射弹丸类型 */;
size_t fric_num; /* 摩擦轮电机数量 */
float ratio_multilevel[MAX_NUM_MULTILEVEL]; /* 多级发射各级速度比例 */
float extra_deceleration_ratio; /*电机出轴到拨盘的额外减速比；没有写1*/
size_t num_trig_tooth; /* 拨弹盘每圈弹丸数量 */
float shot_freq; /* 射击频率，单位Hz */
size_t shot_burst_num; /* 多发模式一次射击的数量 */
}basic;/* 发射基础参数 */
struct {
bool enable; /* 是否启用卡弹检测 */
float threshold; /* 卡弹检测阈值，单位A (dji2006建议设置为120A，dji3508建议设置为235A,根据实际测试调整)*/
float suspectedTime;/* 卡弹怀疑时间，单位秒 */
}jamDetection;/* 卡弹检测参数 */
struct {
bool enable; /* 是否启用热量控制 */
float nmax;//最大射频
float Hwarn;//热量预警值
float Hsatu;//热量饱和值
float Hthres;//热量阈值，超过这个值将无法射击
}heatControl;/* 热量控制参数 */
struct {
Shoot_MOTOR_RM_Param_t fric[MAX_FRIC_NUM];
MOTOR_RM_Param_t trig;
}motor; /* 电机参数 */
struct{
KPID_Params_t fric_follow; /* 摩擦轮电机PID控制参数，用于跟随目标速度 */
KPID_Params_t fric_err; /* 摩擦轮电机PID控制参数，用于消除转速误差 */
KPID_Params_t trig; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg; /* 拨弹电机PID控制参数 */
/* 低通滤波器截止频率 */
KPID_Params_t trig_2006; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg_2006;/* 拨弹电机PID控制参数 */
KPID_Params_t trig_3508; /* 拨弹电机PID控制参数 */
KPID_Params_t trig_omg_3508;/* 拨弹电机PID控制参数 */
}pid; /* PID参数 */
struct {
struct{
float in; /* 反馈值滤波器 */
float out; /* 输出值滤波器 */
float in; /* 反馈值滤波器截止频率 */
float out; /* 输出值滤波器截止频率 */
}fric;
struct{
float in; /* 反馈值滤波器 */
float out; /* 输出值滤波器 */
float in; /* 反馈值滤波器截止频率 */
float out; /* 输出值滤波器截止频率 */
}trig;
} filter;
} filter;/* 滤波器截止频率参数 */
} Shoot_Params_t;
typedef struct {
float now; /* 当前时间，单位秒 */
uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
float dt; /* 两次唤醒间隔时间，单位秒 */
}Shoot_Timer_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
bool online;
float now;
uint64_t lask_wakeup;
float dt;
Shoot_Params_t *param; /* */
Shoot_Timer_t timer; /* 计时器 */
Shoot_Params_t *param; /* 发射参数 */
/* 模块通用 */
Shoot_State_t running_state; /* 运行状态机 */
Shoot_Mode_t mode;
Shoot_Mode_t mode; /* 射击模式 */
/* 反馈信息 */
Shoot_Feedback_t feedback;
/* 控制信息*/
Shoot_AngleCalu_t shoot_Anglecalu;
Shoot_Output_t output;
Shoot_Running_State_t running_state; /* 运行状态机 */
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
Shoot_HeatControl_t heatcontrol; /* 热量控制信息 */
Shoot_VARSForFricCtrl_t var_fric; /* 摩擦轮控制信息 */
Shoot_VARSForTrigCtrl_t var_trig; /* 角度计算控制信息 */
Shoot_Output_t output; /* 输出信息 */
/* 目标控制量 */
struct {
float target_rpm; /* 目标摩擦轮转速 */
float target_angle; /* 目标拨弹位置 */
float fric_rpm; /* 目标摩擦轮转速 */
float trig_angle;/* 目标拨弹位置 */
}target_variable;
/* 反馈控制用的PID */
struct {
KPID_t fric_follow[SHOOT_FRIC_NUM]; /* */
KPID_t fric_err[SHOOT_FRIC_NUM]; /* */
KPID_t trig;
KPID_t trig_omg;
KPID_t fric_follow[MAX_FRIC_NUM];/* 摩擦轮PID主结构体 */
KPID_t fric_err[MAX_FRIC_NUM]; /* 摩擦轮PID主结构体 */
KPID_t trig; /* 拨弹PID主结构体 */
KPID_t trig_omg; /* 拨弹PID主结构体 */
} pid;
/* 滤波器 */
struct {
struct{
LowPassFilter2p_t in[SHOOT_FRIC_NUM]; /* 反馈值滤波器 */
LowPassFilter2p_t out[SHOOT_FRIC_NUM]; /* 输出值滤波器 */
LowPassFilter2p_t in[MAX_FRIC_NUM]; /* 反馈值滤波器 */
LowPassFilter2p_t out[MAX_FRIC_NUM];/* 输出值滤波器 */
}fric;
struct{
LowPassFilter2p_t in; /* 反馈值滤波器 */
LowPassFilter2p_t out; /* 输出值滤波器 */
LowPassFilter2p_t out;/* 输出值滤波器 */
}trig;
} filter;
@ -171,6 +258,16 @@ typedef struct {
*/
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq);
/**
* \brief 设置发射模式
*
* \param s 包含发射数据的结构体
* \param mode 包含发射模式的枚举
*
* \return 函数运行结果
*/
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode);
/**
* \brief 更新反馈
*
@ -190,6 +287,13 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s);
*/
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
/**
* @brief 导出射击UI数据
*
* @param s 射击结构体
* @param ui UI结构体
*/
void Shoot_DumpUI(Shoot_t *s, Shoot_RefereeUI_t *ui);
#ifdef __cplusplus

0
User/device/vision_bridge.c → User/module/vision_bridge.c
View File

0
User/device/vision_bridge.h → User/module/vision_bridge.h
View File

2
User/task/ai.c
View File

@ -9,7 +9,7 @@
#include "bsp/fdcan.h"
#include "module/config.h"
#include "module/gimbal.h"
#include "device/vision_bridge.h"
#include "module/vision_bridge.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */

140
User/task/cmd.c Normal file
View File

@ -0,0 +1,140 @@
/*
cmd Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "module/config.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "module/shoot.h"
#include "module/cmd/cmd.h"
#include "bsp/fdcan.h"
#include "module/vision_bridge.h"
//#define DEAD_AREA 0.05f
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -----------
--------------------------------------------- */
/* USER STRUCT BEGIN */
#if CMD_RCTypeTable_Index == 0
DR16_t cmd_dr16;
#elif CMD_RCTypeTable_Index == 1
AT9S_t cmd_at9s;
#endif
// AI_cmd_t cmd_ai;
#if CMD_ENABLE_SRC_REF
CMD_RawInput_REF_t cmd_ref;
#endif
static CMD_t cmd;
// AI_t ai;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_cmd(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / CMD_FREQ;
osDelay(CMD_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param);
// AI_Init(&ai, &Config_GetRobotParam()->ai_param);
/* 注册CAN接收ID */
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
#if CMD_RCTypeTable_Index == 0
osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_dr16, NULL, 0);
#elif CMD_RCTypeTable_Index == 1
osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0);
#endif
// /* 从CAN2接收AI命令 */
// AI_ParseCmdFromCan( &ai,&cmd_ai);
// #error "弄好自瞄之后统一改"
#if CMD_ENABLE_SRC_REF
/* 从裁判系统读取最新数据（非阻塞） */
osMessageQueueGet(task_runtime.msgq.referee.tocmd.chassis, &cmd_ref.chassis, NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.tocmd.shoot, &cmd_ref.shoot, NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.tocmd.cap, &cmd_ref.cap, NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.tocmd.ai, &cmd_ref.ai, NULL, 0);
#endif
CMD_Update(&cmd);
/* 获取命令发送到各模块 */
#if CMD_ENABLE_MODULE_CHASSIS
Chassis_CMD_t *cmd_for_chassis = CMD_GetChassisCmd(&cmd);
osMessageQueueReset(task_runtime.msgq.chassis.cmd);
osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
#endif
#if CMD_ENABLE_MODULE_GIMBAL
Gimbal_CMD_t *cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0);
#endif
#if CMD_ENABLE_MODULE_SHOOT
Shoot_CMD_t *cmd_for_shoot = CMD_GetShootCmd(&cmd);
osMessageQueueReset(task_runtime.msgq.shoot.cmd);
osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0);
#endif
#if CMD_ENABLE_MODULE_TRACK
Track_CMD_t *cmd_for_track = CMD_GetTrackCmd(&cmd);
osMessageQueueReset(task_runtime.msgq.track.cmd);
osMessageQueuePut(task_runtime.msgq.track.cmd, cmd_for_track, 0, 0);
#endif
#if CMD_ENABLE_MODULE_REFUI
Referee_UI_CMD_t *cmd_for_ui = CMD_GetRefUICmd(&cmd);
osMessageQueueReset(task_runtime.msgq.referee.ui.frcmd);
osMessageQueuePut(task_runtime.msgq.referee.ui.frcmd, cmd_for_ui, 0, 0);
#endif
#if CMD_ENABLE_MODULE_BALANCE_CHASSIS
Chassis_CMD_t *cmd_for_balance_chassis = CMD_GetBalanceChassisCmd(&cmd);
osMessageQueueReset(task_runtime.msgq.chassis.cmd);
osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_balance_chassis, 0, 0);
#endif
#if CMD_ENABLE_SRC_REF
/* 将裁判数据转发到各模块的 .ref 队列 */
{
CMD_RawInput_REF_t *ref = CMD_GetRefData(&cmd);
osMessageQueueReset(task_runtime.msgq.chassis.ref);
osMessageQueuePut(task_runtime.msgq.chassis.ref, &ref->chassis, 0, 0);
osMessageQueueReset(task_runtime.msgq.shoot.ref);
osMessageQueuePut(task_runtime.msgq.shoot.ref, &ref->shoot, 0, 0);
osMessageQueueReset(task_runtime.msgq.cap.ref);
osMessageQueuePut(task_runtime.msgq.cap.ref, &ref->cap, 0, 0);
osMessageQueueReset(task_runtime.msgq.ai.ref);
osMessageQueuePut(task_runtime.msgq.ai.ref, &ref->ai, 0, 0);
}
#endif
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

10
User/task/ctrl_chassis.c
View File

@ -30,6 +30,7 @@ Chassis_CMD_t chassis_cmd = {
.height = 0.0f,
};
Chassis_IMU_t chassis_imu;
Referee_ForChassis_t chassis_ref; /* 裁判系统底盘数据 */
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -95,10 +96,19 @@ void Task_ctrl_chassis(void *argument) {
chassis.feedback.imu = chassis_imu;
}
osMessageQueueGet(task_runtime.msgq.chassis.yaw, &chassis.feedback.yaw, NULL, 0);
osMessageQueueGet(task_runtime.msgq.chassis.ref, &chassis_ref, NULL, 0);
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
// /* 功率限制：裁判系统在线时使用下发上限，否则使用保守默认值 */
// float power_limit = (chassis_ref.ref_status == REF_STATUS_RUNNING)
// ? chassis_ref.chassis_power_limit
// : 500.0f;
// Chassis_Power_Control(&chassis, power_limit);
osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);
/* USER CODE END */

22
User/task/ctrl_shoot.c
View File

@ -9,6 +9,7 @@
#include "device/mrobot.h"
#include "module/shoot.h"
#include "module/config.h"
#include "device/referee_proto_types.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -18,6 +19,7 @@
/* USER STRUCT BEGIN */
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
Referee_ForShoot_t shoot_ref;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -36,10 +38,10 @@ static int print_shoot(const void *data, char *buf, size_t size) {
" Trig : %.1f rpm (angle: %.2f deg)\r\n"
" Output : Fric0=%.1f Fric1=%.1f Trig=%.1f\r\n",
shoot->running_state,
shoot->feedback.fric_rpm[0], shoot->target_variable.target_rpm,
shoot->feedback.fric_rpm[1], shoot->target_variable.target_rpm,
shoot->feedback.fric_avgrpm,
shoot->feedback.trig_rpm, shoot->feedback.trig_angle_cicle,
shoot->feedback.fric[0].rotor_speed, shoot->target_variable.fric_rpm,
shoot->feedback.fric[1].rotor_speed, shoot->target_variable.fric_rpm,
shoot->var_fric.normalized_fil_avgrpm,
shoot->feedback.trig.feedback.rotor_speed, shoot->feedback.trig.feedback.rotor_abs_angle,
shoot->output.out_fric[0], shoot->output.out_fric[1], shoot->output.outagl_trig);
return 0;
}
@ -66,9 +68,17 @@ void Task_ctrl_shoot(void *argument) {
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
osMessageQueueGet(task_runtime.msgq.shoot.shoot_cmd, &shoot_cmd, NULL, 0);
osMessageQueueGet(task_runtime.msgq.shoot.cmd, &shoot_cmd, NULL, 0);
osMessageQueueGet(task_runtime.msgq.shoot.ref, &shoot_ref, NULL, 0);
if (shoot_ref.ref_status == REF_STATUS_RUNNING) {
shoot.heatcontrol.Hmax = (float)shoot_ref.robot_status.shooter_barrel_heat_limit;
shoot.heatcontrol.Hcd = (float)shoot_ref.robot_status.shooter_barrel_cooling_value;
shoot.heatcontrol.Hnow = (float)shoot_ref.power_heat.shooter_42mm_barrel_heat;
shoot.heatcontrol.Hgen = 100.0f; /* 42mm弹丸每发产生热量 */
}
Shoot_UpdateFeedback(&shoot);
// Shoot_Test(&shoot);
Shoot_SetMode(&shoot,shoot_cmd.mode);
Shoot_Control(&shoot,&shoot_cmd);
/* USER CODE END */

26
User/task/init.c
View File

@ -10,6 +10,9 @@
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "device/dr16.h"
#include "device/referee.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -43,11 +46,13 @@ void Task_Init(void *argument) {
// task_runtime.thread.vofa = osThreadNew(Task_vofa, NULL, &attr_vofa);
task_runtime.thread.cli = osThreadNew(Task_cli, NULL, &attr_cli);
// task_runtime.thread.debug = osThreadNew(Task_debug, NULL, &attr_debug);
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
task_runtime.thread.referee = osThreadNew(Task_referee, NULL, &attr_referee);
// 创建消息队列
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
task_runtime.msgq.shoot.shoot_cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
@ -55,6 +60,25 @@ void Task_Init(void *argument) {
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
/* 裁判系统 */
task_runtime.msgq.referee.tocmd.ai= osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
task_runtime.msgq.referee.tocmd.cap= osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
task_runtime.msgq.referee.tocmd.chassis= osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
task_runtime.msgq.referee.tocmd.shoot= osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
/* cmd转发给各模块的裁判数据队列 */
task_runtime.msgq.chassis.ref = osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
task_runtime.msgq.shoot.ref = osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
task_runtime.msgq.ai.ref = osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
task_runtime.msgq.cap.ref = osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
/* UI */
task_runtime.msgq.referee.ui.tochassis =osMessageQueueNew(2u, sizeof(Chassis_RefereeUI_t), NULL);
task_runtime.msgq.referee.ui.tocap =osMessageQueueNew(2u, sizeof(Cap_RefereeUI_t), NULL);
task_runtime.msgq.referee.ui.togimbal =osMessageQueueNew(2u, sizeof(Gimbal_RefereeUI_t), NULL);
task_runtime.msgq.referee.ui.toshoot =osMessageQueueNew(2u, sizeof(Shoot_RefereeUI_t), NULL);
task_runtime.msgq.referee.ui.tocmd = osMessageQueueNew(2u, sizeof(bool), NULL);
task_runtime.msgq.referee.ui.frcmd = osMessageQueueNew(2u, sizeof(Referee_UI_CMD_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

161
User/task/rc.c
View File

@ -8,10 +8,6 @@
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
#include "device/mrobot.h"
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "component/user_math.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -20,9 +16,6 @@
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
DR16_t dr16;
Shoot_CMD_t for_shoot;
Chassis_CMD_t cmd_for_chassis;
Gimbal_CMD_t cmd_for_gimbal;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -74,161 +67,13 @@ void Task_rc(void *argument) {
DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
DR16_ParseData(&dr16);
} else {
DR16_Offline(&dr16);
}
// switch (dr16.data.sw_l) {
// case DR16_SW_UP:
// cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
// break;
// case DR16_SW_MID:
// // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// break;
// case DR16_SW_DOWN:
// // cmd_for_chassis.mode = CHASSIS_MODE_ROTOR;
// // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_JUMP;
// break;
// default:
// cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
// break;
// }
// cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
// cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
// cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
// cmd_for_chassis.height = dr16.data.ch_res;
// osMessageQueueReset(
// task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
// osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
// 0); // 非阻塞发送底盘控制命令
/************************* 云台命令 **************************************/
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
case DR16_SW_MID:
if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
} else {
cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
}
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
case DR16_SW_DOWN:
if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
} else {
cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
}
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
default:
cmd_for_gimbal.mode = GIMBAL_MODE_RELAX;
cmd_for_gimbal.delta_yaw = 0.0f;
cmd_for_gimbal.delta_pit = 0.0f;
break;
}
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);
osMessageQueuePut(task_runtime.msgq.gimbal.cmd, &cmd_for_gimbal, 0, 0);
/************************* 底盘命令 **************************************/
/* 蓄力-释放跳跃逻辑：向上推蓄力收腿，松开回弹时触发跳跃 */
static float last_ch_res = 0.0f; /* 上一次拨杆位置 */
static float min_ch_res = 0.0f; /* 记录最小值（最大蓄力） */
static uint8_t jump_trigger_hold_cnt = 0; /* 触发保持计数 */
static bool in_charge_state = false; /* 是否处于蓄力状态 */
const float CHARGE_THRESHOLD = -0.4f; /* 蓄力阈值：低于此值开始蓄力 */
const float RELEASE_THRESHOLD = -0.2f; /* 释放阈值：回到此值以上触发跳跃 */
const float MIN_CHARGE = -0.5f; /* 最小蓄力量：至少要推到此值才能触发 */
/* 检测蓄力：拨杆向上推 */
if (dr16.data.ch_res < CHARGE_THRESHOLD) {
in_charge_state = true;
if (dr16.data.ch_res < min_ch_res) {
min_ch_res = dr16.data.ch_res; /* 更新最小值 */
}
}
/* 检测释放：拨杆回弹到阈值以上 && 之前有足够蓄力 */
if (in_charge_state &&
dr16.data.ch_res > RELEASE_THRESHOLD &&
min_ch_res < MIN_CHARGE) {
jump_trigger_hold_cnt = 5; /* 触发跳跃，保持5个RC周期 */
in_charge_state = false; /* 退出蓄力状态 */
min_ch_res = 0.0f; /* 重置蓄力状态 */
}
/* 拨杆回到中位，重置蓄力 */
if (dr16.data.ch_res > -0.1f) {
in_charge_state = false;
min_ch_res = 0.0f;
}
cmd_for_chassis.jump_trigger = (jump_trigger_hold_cnt > 0);
if (jump_trigger_hold_cnt > 0) {
jump_trigger_hold_cnt--;
}
last_ch_res = dr16.data.ch_res; /* 保存当前值 */
switch (dr16.data.sw_l) {
case DR16_SW_UP:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
break;
default:
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
}
cmd_for_chassis.move_vec.vx = dr16.data.ch_l_y;
cmd_for_chassis.move_vec.vy = dr16.data.ch_l_x;
cmd_for_chassis.move_vec.wz = dr16.data.ch_r_x;
/* height 传递拨杆位置，负值表示下推收腿，正值表示上推伸腿 */
cmd_for_chassis.height = dr16.data.ch_res;
osMessageQueueReset(
task_runtime.msgq.chassis.cmd); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.cmd, &cmd_for_chassis, 0,
0); // 非阻塞发送底盘控制命令
/************************* 发射命令 **************************************/
for_shoot.online = dr16.header.online;
switch (dr16.data.sw_r) {
case DR16_SW_UP:
for_shoot.ready = false;
for_shoot.firecmd = false;
break;
case DR16_SW_MID:
for_shoot.ready = true;
for_shoot.firecmd = false;
break;
case DR16_SW_DOWN:
for_shoot.ready = true;
for_shoot.firecmd = true;
break;
default:
for_shoot.ready = false;
for_shoot.firecmd = false;
break;
}
osMessageQueueReset(task_runtime.msgq.shoot.shoot_cmd);
osMessageQueuePut(task_runtime.msgq.shoot.shoot_cmd, &for_shoot, 0, 0);
/* 将 DR16 原始数据推入消息队列，cmd 模块负责解析 */
osMessageQueueReset(task_runtime.msgq.cmd.rc);
osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

99
User/task/ref_main.c Normal file
View File

@ -0,0 +1,99 @@
/*
referee Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/referee.h"
#include "module/config.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Referee_t ref;
Referee_UI_t ui;
Referee_UI_CMD_t ref_cmd;
Referee_ForCap_t for_cap;
Referee_ForAI_t for_ai;
Referee_ForChassis_t for_chassis;
Referee_ForShoot_t for_shoot;
uint8_t send_data[6]={1,2,3,4};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* USER PRIVATE CODE BEGIN */
/* USER PRIVATE CODE END */
/* Exported functions ------------------------------------------------------- */
void Task_referee(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / REFEREE_FREQ;
osDelay(REFEREE_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
uint32_t last_online_tick = 0;
/* 初始化裁判系统 */
Referee_Init(&ref, &ui, &Config_GetRobotParam()->ref_screen);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
Referee_StartReceiving(&ref);
if (osThreadFlagsWait(SIGNAL_REFEREE_RAW_REDY, osFlagsWaitAll, 10) !=
SIGNAL_REFEREE_RAW_REDY) {
if (osKernelGetTickCount() - last_online_tick > 2500)
Referee_HandleOffline(&ref);
} else {
Referee_Parse(&ref);
last_online_tick = osKernelGetTickCount();
}
Referee_PackCap(&for_cap, &ref);
Referee_PackAI(&for_ai, &ref);
Referee_PackShoot(&for_shoot, &ref);
Referee_PackChassis(&for_chassis, &ref);
{
/* 裁判系统数据读取 */
osMessageQueueReset(task_runtime.msgq.referee.tocmd.cap);
osMessageQueuePut(task_runtime.msgq.referee.tocmd.cap, &for_cap, 0, 0);
osMessageQueueReset(task_runtime.msgq.referee.tocmd.ai);
osMessageQueuePut(task_runtime.msgq.referee.tocmd.ai, &for_ai, 0, 0);
osMessageQueueReset(task_runtime.msgq.referee.tocmd.chassis);
osMessageQueuePut(task_runtime.msgq.referee.tocmd.chassis, &for_chassis, 0, 0);
osMessageQueueReset(task_runtime.msgq.referee.tocmd.shoot);
osMessageQueuePut(task_runtime.msgq.referee.tocmd.shoot, &for_shoot, 0, 0);
/* UI数据获取 */
osMessageQueueGet(task_runtime.msgq.referee.ui.tocap, &(ui.cap_ui), NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.ui.tochassis, &(ui.chassis_ui), NULL,0);
osMessageQueueGet(task_runtime.msgq.referee.ui.togimbal, &(ui.gimbal_ui), NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.ui.toshoot, &(ui.shoot_ui), NULL, 0);
osMessageQueueGet(task_runtime.msgq.referee.ui.tocmd, &(ui.cmd_pc), NULL, 0);
Referee_UIRefresh(&ui);
while (osMessageQueueGet(task_runtime.msgq.referee.ui.frcmd, &ref_cmd, NULL,
0) == osOK) {
ref_cmd=UI_AUTO_AIM_START;
Referee_PraseCmd(&ui, ref_cmd);
// Referee_StartSend(send_data, sizeof(send_data));
}
Referee_PackUI(&ui, &ref);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

10
User/task/user_task.c
View File

@ -64,3 +64,13 @@ const osThreadAttr_t attr_debug = {
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_cmd = {
.name = "cmd",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_referee = {
.name = "referee",
.priority = osPriorityNormal,
.stack_size = 512 * 4,
};

42
User/task/user_task.h
View File

@ -22,6 +22,8 @@ extern "C" {
#define AI_FREQ (500.0)
#define VOFA_FREQ (500.0)
#define DEBUG_FREQ (10.0)
#define CMD_FREQ (500.0)
#define REFEREE_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
@ -36,6 +38,8 @@ extern "C" {
#define VOFA_INIT_DELAY (0)
#define CLI_INIT_DELAY (0)
#define DEBUG_INIT_DELAY (0)
#define CMD_INIT_DELAY (0)
#define REFEREE_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -56,6 +60,9 @@ typedef struct {
osThreadId_t vofa;
osThreadId_t cli;
osThreadId_t debug;
osThreadId_t cmd;
osThreadId_t referee;
} thread;
/* USER MESSAGE BEGIN */
@ -66,6 +73,7 @@ typedef struct {
osMessageQueueId_t cmd;
osMessageQueueId_t yaw;
osMessageQueueId_t vofa;
osMessageQueueId_t ref;
}chassis;
struct {
osMessageQueueId_t imu;
@ -73,7 +81,8 @@ typedef struct {
osMessageQueueId_t ai_cmd;
}gimbal;
struct {
osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
osMessageQueueId_t cmd; /* 发射命令队列 */
osMessageQueueId_t ref;
}shoot;
struct {
osMessageQueueId_t rc;
@ -85,7 +94,27 @@ typedef struct {
osMessageQueueId_t move_vec;
osMessageQueueId_t eulr;
osMessageQueueId_t fire;
osMessageQueueId_t ref;
}ai;
struct{
osMessageQueueId_t ref; /* Referee_ForCap_t, cmd转发 */
}cap;
struct {
struct {
osMessageQueueId_t cap;
osMessageQueueId_t chassis;
osMessageQueueId_t ai;
osMessageQueueId_t shoot;
} tocmd;
struct {
osMessageQueueId_t tocap;
osMessageQueueId_t tochassis;
osMessageQueueId_t togimbal;
osMessageQueueId_t toshoot;
osMessageQueueId_t tocmd;
osMessageQueueId_t frcmd;
} ui;
}referee;
} msgq;
/* USER MESSAGE END */
@ -113,6 +142,9 @@ typedef struct {
UBaseType_t vofa;
UBaseType_t cli;
UBaseType_t debug;
UBaseType_t cmd;
UBaseType_t referee;
} stack_water_mark;
/* 各任务运行频率 */
@ -126,6 +158,8 @@ typedef struct {
float ai;
float vofa;
float debug;
float cmd;
float referee;
} freq;
/* 任务最近运行时间 */
@ -139,6 +173,8 @@ typedef struct {
float ai;
float vofa;
float debug;
float cmd;
float referee;
} last_up_time;
} Task_Runtime_t;
@ -159,6 +195,8 @@ extern const osThreadAttr_t attr_ai;
extern const osThreadAttr_t attr_vofa;
extern const osThreadAttr_t attr_cli;
extern const osThreadAttr_t attr_debug;
extern const osThreadAttr_t attr_cmd;
extern const osThreadAttr_t attr_referee;
/* 任务函数声明 */
void Task_Init(void *argument);
@ -173,6 +211,8 @@ void Task_ai(void *argument);
void Task_vofa(void *argument);
void Task_cli(void *argument);
void Task_debug(void *argument);
void Task_cmd(void *argument);
void Task_referee(void *argument);
#ifdef __cplusplus
}