xm/god-yuan-hero
xm/god-yuan-hero
1
1
Fork 1
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bc

Browse Source
This commit is contained in:
xxxxm 2026-03-11 22:04:12 +08:00
parent 2a407b4ba0
commit 25eabae70c
33 changed files with 3212 additions and 931 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
CMakeLists.txt
View File

@ -63,7 +63,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/component/pid.c User/component/pid.c
User/component/user_math.c User/component/user_math.c
User/component/crc16.c User/component/crc16.c
User/component/crc8.c
User/component/ui.c
# User/component/ahrs sources # User/component/ahrs sources
User/component/ahrs/ahrs.c User/component/ahrs/ahrs.c
@ -79,6 +80,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_rm.c User/device/motor_rm.c
User/device/supercap.c User/device/supercap.c
User/device/gimbal_imu.c User/device/gimbal_imu.c
User/device/referee.c
# User/module sources # User/module sources
User/module/chassis.c User/module/chassis.c
@ -106,6 +108,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/supercap.c User/task/supercap.c
User/task/user_task.c User/task/user_task.c
User/task/ai.c User/task/ai.c
User/task/referee.c
) )
# Add include paths # Add include paths

4
User/bsp/uart.c
View File

@ -27,7 +27,7 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
return BSP_UART_RC; return BSP_UART_RC;
else else
if (huart->Instance == USART10) if (huart->Instance == USART10)
return BSP_UART_AI; return BSP_UART_REF;
else else
return BSP_UART_ERR; return BSP_UART_ERR;
} }
@ -118,7 +118,7 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) { switch (uart) {
case BSP_UART_RC: case BSP_UART_RC:
return &huart5; return &huart5;
case BSP_UART_AI: case BSP_UART_REF:
return &huart10; return &huart10;
default: default:
return NULL; return NULL;

1
User/bsp/uart.h
View File

@ -29,6 +29,7 @@ extern "C" {
typedef enum { typedef enum {
BSP_UART_RC, BSP_UART_RC,
BSP_UART_AI, BSP_UART_AI,
BSP_UART_REF,
BSP_UART_NUM, BSP_UART_NUM,
BSP_UART_ERR, BSP_UART_ERR,
} BSP_UART_t; } BSP_UART_t;

52
User/component/crc8.c Normal file
View File

@ -0,0 +1,52 @@
#include "crc8.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
User/component/crc8.h Normal file
View File

@ -0,0 +1,30 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

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

8
User/device/device.h
View File

@ -27,6 +27,11 @@ extern "C" {
#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2) #define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3) #define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3)
#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4) #define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4)
#define SIGNAL_AT9S_RAW_REDY (1u << 7)
#define SIGNAL_VT13_RAW_REDY (1u << 8)
#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
/* AUTO GENERATED SIGNALS END */ /* AUTO GENERATED SIGNALS END */
/* USER SIGNALS BEGIN */ /* USER SIGNALS BEGIN */
@ -39,8 +44,7 @@ typedef struct {
} DEVICE_Header_t; } DEVICE_Header_t;
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
#define SIGNAL_AT9S_RAW_REDY (1u << 7)
#define SIGNAL_VT13_RAW_REDY (1u << 8)
/* USER STRUCT END */ /* USER STRUCT END */
/* USER FUNCTION BEGIN */ /* USER FUNCTION 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];
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,true)
== 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.4 + sin(ui->chassis_ui.angle) * 46,
ui->screen->height * 0.2 + cos(ui->chassis_ui.angle) * 46);
float start_pos_h = 0.0f;
switch (ui->chassis_ui.mode) {
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_BREAK:
start_pos_h = 0.68f;
break;
case CHASSIS_MODE_FOLLOW_GIMBAL:
start_pos_h = 0.66f;
break;
case CHASSIS_MODE_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;
}

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

@ -0,0 +1,681 @@
/*
裁判系统抽象。
*/
#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\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;
/* Referee_ChassisUI_t 已移至 module/chassis.h */
/* Referee_CapUI_t 已移至 device/supercap.h */
/* Referee_GimbalUI_t 已移至 module/gimbal.h */
/* Referee_ShootUI_t 已移至 module/shoot.h */
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所需信息 */
Referee_CapUI_t cap_ui;
Referee_ChassisUI_t chassis_ui;
Referee_ShootUI_t shoot_ui;
Referee_GimbalUI_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

8
User/device/supercap.h
View File

@ -27,7 +27,7 @@ typedef enum
BAT_UNDER_VOLTAGE_PROTECTION =6, //电池欠压保护，电池要没电了，换电池 BAT_UNDER_VOLTAGE_PROTECTION =6, //电池欠压保护，电池要没电了，换电池
CAP_UNDER_VOLTAGE_PROTECTION =7, //超级电容欠压保护，超级电容用完电了，要充一会才能用 CAP_UNDER_VOLTAGE_PROTECTION =7, //超级电容欠压保护，超级电容用完电了，要充一会才能用
OVER_TEMPERATURE_PROTECTION =8, //过温保护，太热了 OVER_TEMPERATURE_PROTECTION =8, //过温保护，太热了
BOOM = 9, //超电爆炸了 BOOM = 9, //超电爆炸了
}SuperCapStateEnum; }SuperCapStateEnum;
//超级电容准备状态，用于判断超级电容是否可以使用 //超级电容准备状态，用于判断超级电容是否可以使用
@ -97,6 +97,12 @@ uint32_t get_chassis_energy_from_supercap(void);
int8_t SuperCap_Init(void); int8_t SuperCap_Init(void);
int8_t SuperCap_Update(void); int8_t SuperCap_Update(void);
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
float percentage;
SuperCap_Status_t status;
} Referee_CapUI_t;
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif /*SUPERCAP_H*/ #endif /*SUPERCAP_H*/

11
User/module/chassis.c
View File

@ -311,4 +311,13 @@ void Chassis_ResetOutput(Chassis_t *c) {
} }
} }
/**
* @brief 导出底盘数据
*
* @param chassis 底盘数据结构体
* @param ui UI数据结构体
*/
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui) {
ui->mode = c->mode;
ui->angle = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
}

7
User/module/chassis.h
View File

@ -42,6 +42,12 @@ typedef enum {
ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */ ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */
} Chassis_RotorMode_t; } Chassis_RotorMode_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
Chassis_Mode_t mode;
float angle;
} Referee_ChassisUI_t;
/* µ×ÅÌ¿ØÖÆÃüÁî */ /* µ×ÅÌ¿ØÖÆÃüÁî */
typedef struct { typedef struct {
Chassis_Mode_t mode; /* µ×ÅÌÔËÐÐģʽ */ Chassis_Mode_t mode; /* µ×ÅÌÔËÐÐģʽ */
@ -229,6 +235,7 @@ void Chassis_Power_Control(Chassis_t *c,float max_power);
*/ */
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui); //void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus #ifdef __cplusplus
} }

316
User/module/cmd/cmd.c
View File

@ -11,6 +11,7 @@
/* ========================================================================== */ /* ========================================================================== */
/* 从RC输入生成底盘命令 */ /* 从RC输入生成底盘命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_RC_BuildChassisCmd(CMD_t *ctx) { static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
@ -33,9 +34,12 @@ static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
/* 摇杆控制移动 */ /* 摇杆控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x; 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.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输入生成云台命令 */ /* 从 RC 输入生成云台命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
ctx->output.gimbal.cmd.is_ai = false; ctx->output.gimbal.cmd.is_ai = false;
@ -59,8 +63,10 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f; ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f;
ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f; ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f;
} }
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */
/* 从RC输入生成射击命令 */ /* 从 RC 输入生成射击命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT
static void CMD_RC_BuildShootCmd(CMD_t *ctx) { static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
if (ctx->input.online[CMD_SRC_RC]) { if (ctx->input.online[CMD_SRC_RC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE;
@ -85,8 +91,10 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
break; break;
} }
} }
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */
/* 从PC输入生成射击命令 */ /* 从 RC 输入生成履带命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK
static void CMD_RC_BuildTrackCmd(CMD_t *ctx) { static void CMD_RC_BuildTrackCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
@ -118,8 +126,10 @@ static void CMD_RC_BuildTrackCmd(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK); CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
} }
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK */
/* 从PC输入生成底盘命令 */ /* 从PC输入生成底盘命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_PC_BuildChassisCmd(CMD_t *ctx) { static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) { if (!ctx->input.online[CMD_SRC_PC]) {
@ -134,8 +144,10 @@ static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f; ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS); CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS);
} }
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */
/* 从PC输入生成云台命令 */ /* 从PC输入生成云台命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) { static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
CMD_Sensitivity_t *sens = &ctx->config->sensitivity; CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
@ -151,8 +163,10 @@ static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
ctx->output.gimbal.cmd.delta_pit = (float)ctx->input.pc.mouse.y * ctx->timer.dt * sens->mouse_sens * 1.5f; 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); CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_GIMBAL);
} }
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL */
/* 从PC输入生成射击命令 */ /* 从PC输入生成射击命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT
static void CMD_PC_BuildShootCmd(CMD_t *ctx) { static void CMD_PC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) { if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
@ -165,8 +179,10 @@ static void CMD_PC_BuildShootCmd(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT); CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT);
} }
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */
/* 从PC输入生成履带命令 */ /* 从PC输入生成履带命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK
static void CMD_PC_BuildTrackCmd(CMD_t *ctx) { static void CMD_PC_BuildTrackCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) { if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.track.cmd.enable = false; ctx->output.track.cmd.enable = false;
@ -182,8 +198,10 @@ static void CMD_PC_BuildTrackCmd(CMD_t *ctx) {
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK); CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
} }
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK */
/* 从NUC/AI输入生成云台命令 */ /* 从NUC/AI输入生成云台命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) { static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) { if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
@ -200,8 +218,10 @@ static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
} }
} }
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */
/* 从NUC/AI输入生成射击命令 */ /* 从 NUC/AI 输入生成射击命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT
static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) { if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
@ -230,13 +250,138 @@ static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
break; 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 */
/* 裁判系统UI命令构建 - NUC自瞄状态 */
#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) { static void CMD_SetOfflineMode(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.cmd.enable = false; ctx->output.track.cmd.enable = false;
#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
} }
/* ========================================================================== */ /* ========================================================================== */
@ -282,13 +427,90 @@ typedef struct {
CMD_BuildCommandFunc gimbalFunc; CMD_BuildCommandFunc gimbalFunc;
CMD_BuildCommandFunc shootFunc; CMD_BuildCommandFunc shootFunc;
CMD_BuildCommandFunc trackFunc; CMD_BuildCommandFunc trackFunc;
CMD_BuildCommandFunc armFunc;
CMD_BuildCommandFunc refuiFunc;
} CMD_SourceHandler_t; } 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
#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_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] = { CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
{CMD_SRC_RC, CMD_RC_BuildChassisCmd, CMD_RC_BuildGimbalCmd, CMD_RC_BuildShootCmd, CMD_RC_BuildTrackCmd}, #if CMD_ENABLE_SRC_RC
{CMD_SRC_PC, CMD_PC_BuildChassisCmd, CMD_PC_BuildGimbalCmd, CMD_PC_BuildShootCmd, CMD_PC_BuildTrackCmd}, [CMD_SRC_RC] = {CMD_SRC_RC, _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK, _FN_RC_ARM, NULL},
{CMD_SRC_NUC, NULL, CMD_NUC_BuildGimbalCmd, CMD_NUC_BuildShootCmd, NULL}, #endif
{CMD_SRC_REF, NULL, NULL, NULL, NULL}, #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},
#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},
#endif
#if CMD_ENABLE_SRC_REF
[CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL, NULL, NULL},
#endif
}; };
int8_t CMD_Arbitrate(CMD_t *ctx) { int8_t CMD_Arbitrate(CMD_t *ctx) {
@ -296,14 +518,22 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
return CMD_ERR_NULL; return CMD_ERR_NULL;
} }
/* 自动仲裁：优先级 PC > RC > NUC */ /* RC > PC priority arbitration */
// CMD_InputSource_t candidates[] = {CMD_SRC_RC, CMD_SRC_PC, CMD_SRC_NUC}; CMD_InputSource_t candidates[] = {
CMD_InputSource_t candidates[] = {CMD_SRC_RC, CMD_SRC_PC}; #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]); const int num_candidates = sizeof(candidates) / sizeof(candidates[0]);
/* 如果当前输入源仍然在线且有效，保持使用 */ /* keep current source if still online */
if (ctx->active_source < CMD_SRC_NUM && if (ctx->active_source < CMD_SRC_NUM &&
ctx->active_source != CMD_SRC_REF && #if CMD_ENABLE_SRC_REF
ctx->active_source != CMD_SRC_REF &&
#endif
ctx->input.online[ctx->active_source]) { ctx->input.online[ctx->active_source]) {
goto seize; goto seize;
} }
@ -323,22 +553,41 @@ int8_t CMD_Arbitrate(CMD_t *ctx) {
/* 优先级抢占逻辑 */ /* 优先级抢占逻辑 */
seize: seize:
/* 重置输入源 */ /* reset output sources */
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = ctx->active_source; ctx->output.chassis.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = ctx->active_source; ctx->output.gimbal.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = ctx->active_source; ctx->output.shoot.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.source = ctx->active_source; 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
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE); 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->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {//保护其余模式下安全 if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) { if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
ctx->output.gimbal.source = CMD_SRC_NUC; ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.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; return CMD_OK;
} }
@ -360,20 +609,46 @@ int8_t CMD_GenerateCommands(CMD_t *ctx) {
return CMD_ERR_NO_INPUT; return CMD_ERR_NO_INPUT;
} }
#if CMD_ENABLE_MODULE_GIMBAL
if (sourceHandlers[ctx->output.gimbal.source].gimbalFunc != NULL) { if (sourceHandlers[ctx->output.gimbal.source].gimbalFunc != NULL) {
sourceHandlers[ctx->output.gimbal.source].gimbalFunc(ctx); sourceHandlers[ctx->output.gimbal.source].gimbalFunc(ctx);
} }
#endif
#if CMD_ENABLE_MODULE_CHASSIS
if (sourceHandlers[ctx->output.chassis.source].chassisFunc != NULL) { if (sourceHandlers[ctx->output.chassis.source].chassisFunc != NULL) {
sourceHandlers[ctx->output.chassis.source].chassisFunc(ctx); sourceHandlers[ctx->output.chassis.source].chassisFunc(ctx);
} }
#endif
#if CMD_ENABLE_MODULE_SHOOT
if (sourceHandlers[ctx->output.shoot.source].shootFunc != NULL) { if (sourceHandlers[ctx->output.shoot.source].shootFunc != NULL) {
sourceHandlers[ctx->output.shoot.source].shootFunc(ctx); sourceHandlers[ctx->output.shoot.source].shootFunc(ctx);
} }
#endif
#if CMD_ENABLE_MODULE_TRACK
if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) { if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) {
sourceHandlers[ctx->output.track.source].trackFunc(ctx); 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
return CMD_OK; 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 CMD_Update(CMD_t *ctx) {
int8_t ret; int8_t ret;
@ -384,5 +659,10 @@ int8_t CMD_Update(CMD_t *ctx) {
CMD_Arbitrate(ctx); CMD_Arbitrate(ctx);
ret = CMD_GenerateCommands(ctx); ret = CMD_GenerateCommands(ctx);
#if CMD_ENABLE_SRC_REF
CMD_REF_BuildOutput(ctx);
#endif
return ret; return ret;
} }

103
User/module/cmd/cmd.h
View File

@ -8,11 +8,23 @@
#include "cmd_adapter.h" #include "cmd_adapter.h"
#include "cmd_behavior.h" #include "cmd_behavior.h"
/* 引入输出模块的命令类型 */ /* 按需引入输出模块的命令类型 */
#include "module/chassis.h" #if CMD_ENABLE_MODULE_CHASSIS
#include "module/gimbal.h" #include "module/chassis.h"
#include "module/shoot.h" #endif
#include "module/track.h" #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
#include <stdint.h> #include <stdint.h>
#ifdef __cplusplus #ifdef __cplusplus
@ -24,25 +36,52 @@ extern "C" {
/* ========================================================================== */ /* ========================================================================== */
/* 每个模块的输出包含源信息和命令 */ /* 每个模块的输出包含源信息和命令 */
#if CMD_ENABLE_MODULE_CHASSIS
typedef struct { typedef struct {
CMD_InputSource_t source; CMD_InputSource_t source;
Chassis_CMD_t cmd; Chassis_CMD_t cmd;
} CMD_ChassisOutput_t; } CMD_ChassisOutput_t;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
typedef struct { typedef struct {
CMD_InputSource_t source; CMD_InputSource_t source;
Gimbal_CMD_t cmd; Gimbal_CMD_t cmd;
} CMD_GimbalOutput_t; } CMD_GimbalOutput_t;
#endif
#if CMD_ENABLE_MODULE_SHOOT
typedef struct { typedef struct {
CMD_InputSource_t source; CMD_InputSource_t source;
Shoot_CMD_t cmd; Shoot_CMD_t cmd;
} CMD_ShootOutput_t; } CMD_ShootOutput_t;
#endif
#if CMD_ENABLE_MODULE_TRACK
typedef struct { typedef struct {
CMD_InputSource_t source; CMD_InputSource_t source;
Track_CMD_t cmd; Track_CMD_t cmd;
} CMD_TrackOutput_t; } 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_SRC_REF
// /* REF 透传输出：裁判数据直通各模块，不参与仲裁 */
// typedef CMD_RawInput_REF_t CMD_RawInput_REF_t;
#endif
/* ========================================================================== */ /* ========================================================================== */
/* 配置结构 */ /* 配置结构 */
@ -58,20 +97,26 @@ typedef struct {
/* RC模式映射配置 - 定义开关位置到模式的映射 */ /* RC模式映射配置 - 定义开关位置到模式的映射 */
typedef struct { typedef struct {
#if CMD_ENABLE_MODULE_CHASSIS
/* 左拨杆映射 - 底盘模式 */ /* 左拨杆映射 - 底盘模式 */
Chassis_Mode_t sw_left_up; Chassis_Mode_t sw_left_up;
Chassis_Mode_t sw_left_mid; Chassis_Mode_t sw_left_mid;
Chassis_Mode_t sw_left_down; Chassis_Mode_t sw_left_down;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
/* 右拨杆映射 - 云台/射击模式 */ /* 右拨杆映射 - 云台/射击模式 */
Gimbal_Mode_t gimbal_sw_up; Gimbal_Mode_t gimbal_sw_up;
Gimbal_Mode_t gimbal_sw_mid; Gimbal_Mode_t gimbal_sw_mid;
Gimbal_Mode_t gimbal_sw_down; Gimbal_Mode_t gimbal_sw_down;
#endif
#if CMD_ENABLE_MODULE_TRACK
/* 左拨杆映射 - 履带使能 */ /* 左拨杆映射 - 履带使能 */
bool track_sw_up; bool track_sw_up;
bool track_sw_mid; bool track_sw_mid;
bool track_sw_down; bool track_sw_down;
#endif
} CMD_RCModeMap_t; } CMD_RCModeMap_t;
/* 整体配置 */ /* 整体配置 */
@ -113,10 +158,27 @@ typedef struct CMD_Context {
/* 输出 */ /* 输出 */
struct { struct {
#if CMD_ENABLE_MODULE_CHASSIS
CMD_ChassisOutput_t chassis; CMD_ChassisOutput_t chassis;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
CMD_GimbalOutput_t gimbal; CMD_GimbalOutput_t gimbal;
#endif
#if CMD_ENABLE_MODULE_SHOOT
CMD_ShootOutput_t shoot; CMD_ShootOutput_t shoot;
#endif
#if CMD_ENABLE_MODULE_TRACK
CMD_TrackOutput_t 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_SRC_REF
CMD_RawInput_REF_t ref;
#endif
} output; } output;
} CMD_t; } CMD_t;
@ -165,24 +227,53 @@ int8_t CMD_Update(CMD_t *ctx);
/* ========================================================================== */ /* ========================================================================== */
/* 获取底盘命令 */ /* 获取底盘命令 */
#if CMD_ENABLE_MODULE_CHASSIS
static inline Chassis_CMD_t* CMD_GetChassisCmd(CMD_t *ctx) { static inline Chassis_CMD_t* CMD_GetChassisCmd(CMD_t *ctx) {
return &ctx->output.chassis.cmd; return &ctx->output.chassis.cmd;
} }
#endif
/* 获取云台命令 */ /* 获取云台命令 */
#if CMD_ENABLE_MODULE_GIMBAL
static inline Gimbal_CMD_t* CMD_GetGimbalCmd(CMD_t *ctx) { static inline Gimbal_CMD_t* CMD_GetGimbalCmd(CMD_t *ctx) {
return &ctx->output.gimbal.cmd; return &ctx->output.gimbal.cmd;
} }
#endif
/* 获取射击命令 */ /* 获取射击命令 */
#if CMD_ENABLE_MODULE_SHOOT
static inline Shoot_CMD_t* CMD_GetShootCmd(CMD_t *ctx) { static inline Shoot_CMD_t* CMD_GetShootCmd(CMD_t *ctx) {
return &ctx->output.shoot.cmd; return &ctx->output.shoot.cmd;
} }
#endif
/* 获取履带命令 */ /* 获取履带命令 */
#if CMD_ENABLE_MODULE_TRACK
static inline Track_CMD_t* CMD_GetTrackCmd(CMD_t *ctx) { static inline Track_CMD_t* CMD_GetTrackCmd(CMD_t *ctx) {
return &ctx->output.track.cmd; 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_SRC_REF
static inline CMD_RawInput_REF_t* CMD_GetRefData(CMD_t *ctx) {
return &ctx->output.ref;
}
#endif
#ifdef __cplusplus #ifdef __cplusplus
} }

39
User/module/cmd/cmd_adapter.c
View File

@ -124,6 +124,34 @@ CMD_DEFINE_ADAPTER(AT9S, at9s, CMD_SRC_RC, CMD_AT9S_Init, CMD_AT9S_GetInput, CMD
/* ========================================================================== */ /* ========================================================================== */
/* NUC/AI 适配器实现 */ /* 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] = true;
output->ref = *ref;
return CMD_OK;
}
bool CMD_REF_IsOnline(void *data) {
(void)data;
return true;
}
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) { int8_t CMD_NUC_AdapterInit(void *data) {
/* NUC适配器不需要特殊初始化 */ /* NUC适配器不需要特殊初始化 */
return CMD_OK; return CMD_OK;
@ -154,6 +182,8 @@ bool CMD_NUC_IsOnline(void *data) {
extern AI_cmd_t ai_cmd; extern AI_cmd_t ai_cmd;
CMD_DEFINE_ADAPTER(NUC, cmd_ai, CMD_SRC_NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput, CMD_NUC_IsOnline) CMD_DEFINE_ADAPTER(NUC, cmd_ai, CMD_SRC_NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput, CMD_NUC_IsOnline)
#endif /* CMD_ENABLE_SRC_NUC */
/* ========================================================================== */ /* ========================================================================== */
/* 适配器管理实现 */ /* 适配器管理实现 */
/* ========================================================================== */ /* ========================================================================== */
@ -176,9 +206,16 @@ int8_t CMD_Adapter_InitAll(void) {
/* AT9S 目前只支持 RC 输入 */ /* AT9S 目前只支持 RC 输入 */
CMD_Adapter_Register(&g_adapter_AT9S); CMD_Adapter_Register(&g_adapter_AT9S);
#endif #endif
#if CMD_ENABLE_SRC_NUC
/* 注册NUC适配器 */ /* 注册NUC适配器 */
CMD_Adapter_Register(&g_adapter_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++) { for (int i = 0; i < CMD_SRC_NUM; i++) {

22
User/module/cmd/cmd_adapter.h
View File

@ -92,11 +92,23 @@ typedef struct {
/* ========================================================================== */ /* ========================================================================== */
/* NUC/AI适配器配置 */ /* NUC/AI适配器配置 */
/* ========================================================================== */ /* ========================================================================== */
#include "module/vision_bridge.h" #if CMD_ENABLE_SRC_NUC
extern AI_cmd_t cmd_ai; #include "module/vision_bridge.h"
int8_t CMD_NUC_AdapterInit(void *data); extern AI_cmd_t cmd_ai;
int8_t CMD_NUC_GetInput(void *data, CMD_RawInput_t *output); int8_t CMD_NUC_AdapterInit(void *data);
bool CMD_NUC_IsOnline(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
/* ========================================================================== */ /* ========================================================================== */
/* 适配器管理接口 */ /* 适配器管理接口 */

51
User/module/cmd/cmd_behavior.c
View File

@ -3,7 +3,9 @@
*/ */
#include "cmd_behavior.h" #include "cmd_behavior.h"
#include "cmd.h" #include "cmd.h"
#include "module/gimbal.h" #if CMD_ENABLE_MODULE_GIMBAL
#include "module/gimbal.h"
#endif
#include <string.h> #include <string.h>
/* ========================================================================== */ /* ========================================================================== */
@ -12,77 +14,120 @@
/* 行为处理函数实现 */ /* 行为处理函数实现 */
int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) { 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; ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) { 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; ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) { 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; ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) { 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; ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) { 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.vx *= ctx->config->sensitivity.move_fast_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult; ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) { 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.vx *= ctx->config->sensitivity.move_slow_mult;
ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult; ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_FIRE(CMD_t *ctx) { int8_t CMD_Behavior_Handle_FIRE(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.firecmd = true; ctx->output.shoot.cmd.firecmd = true;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) { 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; ctx->output.shoot.cmd.mode = (ctx->output.shoot.cmd.mode + 1) % SHOOT_MODE_NUM;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) { 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 = CHASSIS_MODE_ROTOR;
ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND; ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
#endif
return CMD_OK; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_AUTOAIM(CMD_t *ctx) { int8_t CMD_Behavior_Handle_AUTOAIM(CMD_t *ctx) {
/* TODO: 自瞄模式切换 */ /* 自瞄模式切换 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) { if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source == CMD_SRC_PC){ if (ctx->active_source == CMD_SRC_PC){
ctx->output.gimbal.source = CMD_SRC_NUC; ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.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; return CMD_OK;
} }
int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) { int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) {
/* TODO: 切换RC和PC输入源 */ /* 切换RC和PC输入源 */
if (ctx->active_source == CMD_SRC_PC) { if (ctx->active_source == CMD_SRC_PC) {
ctx->active_source = CMD_SRC_RC; ctx->active_source = CMD_SRC_RC;
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = CMD_SRC_RC; ctx->output.chassis.source = CMD_SRC_RC;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = CMD_SRC_RC; ctx->output.gimbal.source = CMD_SRC_RC;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = CMD_SRC_RC; 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) { } else if(ctx->active_source == CMD_SRC_RC) {
ctx->active_source = CMD_SRC_PC; ctx->active_source = CMD_SRC_PC;
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = CMD_SRC_PC; ctx->output.chassis.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = CMD_SRC_PC; ctx->output.gimbal.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = CMD_SRC_PC; ctx->output.shoot.source = CMD_SRC_PC;
#endif
#if CMD_ENABLE_MODULE_ARM
ctx->output.arm.source = CMD_SRC_PC;
#endif
} }
return CMD_OK; return CMD_OK;
} }

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

@ -0,0 +1,63 @@
/*
* 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 1
/**
* 裁判系统数据中转开关
* 1 (比赛模式): cmd 将 referee 数据转发到各模块的 .ref 队列
* 0 (调试模式): cmd 不转发，裁判系统可断开，不影响其他功能
*/
#define CMD_ENABLE_SRC_REF 1
/* ========================================================================== */
/* 输出模块使能开关 */
/* ========================================================================== */
/** 底盘模块 (需要 module/chassis.h) */
#define CMD_ENABLE_MODULE_CHASSIS 1
/** 云台模块 (需要 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 1
/** 机械臂模块 (需要 component/arm_kinematics/arm6dof.h) */
#define CMD_ENABLE_MODULE_ARM 0
/** 裁判系统UI命令模块 (需要 device/referee.h) */
#define CMD_ENABLE_MODULE_REFUI 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 */

114
User/module/cmd/cmd_types.h
View File

@ -6,6 +6,8 @@
#include <stdint.h> #include <stdint.h>
#include <stdbool.h> #include <stdbool.h>
#include "cmd_feature.h" /* 功能特性开关 */
#include "device/referee_proto_types.h" /* 裁判转发包基础类型（叶节点，无循环依赖） */
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
@ -23,21 +25,87 @@ extern "C" {
/* ========================================================================== */ /* ========================================================================== */
/* 输入源配置宏表 */ /* 输入源配置宏表 */
/* ========================================================================== */ /* ========================================================================== */
/* /*
* 使用方法：在config中定义需要启用的输入源 * 使用方法：在 cmd_feature.h 中设置各 CMD_ENABLE_SRC_xxx 宏。
* 格式: X(枚举名, 优先级, 适配器初始化函数, 获取数据函数) * 格式: 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) \ #define CMD_INPUT_SOURCE_TABLE(X) \
X(RC, CMD_RC_AdapterInit, CMD_RC_GetInput) \ _X_SRC_RC(X) \
X(PC, CMD_PC_AdapterInit, CMD_PC_GetInput) \ _X_SRC_PC(X) \
X(NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput) \ _X_SRC_NUC(X) \
X(REF, CMD_REF_AdapterInit, CMD_REF_GetInput) _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
/* 输出模块配置宏表 */ /* 输出模块配置宏表 */
#define CMD_OUTPUT_MODULE_TABLE(X) \ #define CMD_OUTPUT_MODULE_TABLE(X) \
X(CHASSIS, Chassis_CMD_t, chassis) \ _X_MOD_CHASSIS(X) \
X(GIMBAL, Gimbal_CMD_t, gimbal) \ _X_MOD_GIMBAL(X) \
X(SHOOT, Shoot_CMD_t, shoot) \ _X_MOD_SHOOT(X) \
_X_MOD_TRACK(X) \
_X_MOD_ARM(X) \
_X_MOD_REFUI(X)
/* ========================================================================== */ /* ========================================================================== */
@ -92,11 +160,6 @@ typedef enum {
CMD_KEY_NUM CMD_KEY_NUM
} CMD_KeyIndex_t; } CMD_KeyIndex_t;
/* 裁判系统数据 */
typedef struct {
uint8_t game_status; /* 比赛状态 */
} CMD_Referee_t;
typedef struct { typedef struct {
CMD_Joystick_t joy_left; /* 左摇杆 */ CMD_Joystick_t joy_left; /* 左摇杆 */
CMD_Joystick_t joy_right; /* 右摇杆 */ CMD_Joystick_t joy_right; /* 右摇杆 */
@ -128,25 +191,39 @@ typedef struct {
} gimbal; } gimbal;
} CMD_RawInput_NUC_t; } CMD_RawInput_NUC_t;
#if CMD_ENABLE_SRC_REF
/* 裁判系统原始输入，包含需转发给各模块的完整子集 */
typedef struct { typedef struct {
CMD_Referee_t referee; Referee_ForChassis_t chassis;
Referee_ForShoot_t shoot;
Referee_ForCap_t cap;
Referee_ForAI_t ai;
} CMD_RawInput_REF_t; } CMD_RawInput_REF_t;
#endif
/* 统一的原始输入结构 - 所有设备适配后都转换成这个格式 */ /* 统一的原始输入结构 - 所有设备适配后都转换成这个格式 */
typedef struct { typedef struct {
bool online[CMD_SRC_NUM]; bool online[CMD_SRC_NUM];
#if CMD_ENABLE_SRC_RC
/* 遥控器部分 */ /* 遥控器部分 */
CMD_RawInput_RC_t rc; CMD_RawInput_RC_t rc;
#endif
#if CMD_ENABLE_SRC_PC
/* PC部分 */ /* PC部分 */
CMD_RawInput_PC_t pc; CMD_RawInput_PC_t pc;
#endif
#if CMD_ENABLE_SRC_NUC
/* NUC部分 */ /* NUC部分 */
CMD_RawInput_NUC_t nuc; CMD_RawInput_NUC_t nuc;
#endif
#if CMD_ENABLE_SRC_REF
/* REF部分 - 裁判系统数据 */ /* REF部分 - 裁判系统数据 */
CMD_RawInput_REF_t ref; CMD_RawInput_REF_t ref;
#endif
} CMD_RawInput_t; } CMD_RawInput_t;
/* ========================================================================== */ /* ========================================================================== */
@ -158,7 +235,9 @@ typedef enum {
CMD_MODULE_GIMBAL = (1 << 2), CMD_MODULE_GIMBAL = (1 << 2),
CMD_MODULE_SHOOT = (1 << 3), CMD_MODULE_SHOOT = (1 << 3),
CMD_MODULE_TRACK = (1 << 4), CMD_MODULE_TRACK = (1 << 4),
CMD_MODULE_ALL = 0x1E CMD_MODULE_ARM = (1 << 5),
CMD_MODULE_REFUI = (1 << 6),
CMD_MODULE_ALL = 0x7E
} CMD_ModuleMask_t; } CMD_ModuleMask_t;
@ -207,6 +286,7 @@ typedef enum {
#define CMD_KEY_SET(kb, key) ((kb)->bitmap |= (1 << (key))) #define CMD_KEY_SET(kb, key) ((kb)->bitmap |= (1 << (key)))
#define CMD_KEY_CLEAR(kb, key) ((kb)->bitmap &= ~(1 << (key))) #define CMD_KEY_CLEAR(kb, key) ((kb)->bitmap &= ~(1 << (key)))
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

6
User/module/config.h
View File

@ -20,14 +20,16 @@ extern "C" {
#include "component/PowerControl.h" #include "component/PowerControl.h"
#include "device/gimbal_imu.h" #include "device/gimbal_imu.h"
#include "module/vision_bridge.h" #include "module/vision_bridge.h"
typedef struct { typedef struct {
Shoot_Params_t shoot_param; Shoot_Params_t shoot_param;
Gimbal_Params_t gimbal_param; Gimbal_Params_t gimbal_param;
Chassis_Params_t chassis_param; Chassis_Params_t chassis_param;
Track_Params_t track_param; Track_Params_t track_param;
CMD_Config_t cmd_param; CMD_Config_t cmd_param;
GIMBAL_IMU_Param_t imu_param; GIMBAL_IMU_Param_t imu_param;
AI_Param_t ai_param; AI_Param_t ai_param;
Referee_Screen_t ref_screen;
} Config_RobotParam_t; } Config_RobotParam_t;
extern power_model_t cha; extern power_model_t cha;

10
User/module/gimbal.c
View File

@ -303,3 +303,13 @@ void Gimbal_Output(Gimbal_t *g){
output_yaw.kd = 0.6f; output_yaw.kd = 0.6f;
MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output_yaw); MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output_yaw);
} }
/**
* @brief 导出云台UI数据
*
* @param g 云台结构体
* @param ui UI结构体
*/
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui) {
ui->mode = g->mode;
}

6
User/module/gimbal.h
View File

@ -32,6 +32,11 @@ typedef enum {
GIMBAL_MODE_NUM GIMBAL_MODE_NUM
} Gimbal_Mode_t; } Gimbal_Mode_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
Gimbal_Mode_t mode;
} Referee_GimbalUI_t;
typedef struct { typedef struct {
Gimbal_Mode_t mode; Gimbal_Mode_t mode;
float delta_yaw; float delta_yaw;
@ -198,6 +203,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
*/ */
void Gimbal_Output(Gimbal_t *g); void Gimbal_Output(Gimbal_t *g);
void Gimbal_DumpUI(const Gimbal_t *g, Referee_GimbalUI_t *ui);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

26
User/module/shoot.c
View File

@ -351,8 +351,8 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
float target_rpm=s->param->basic.ratio_multilevel[level] float target_rpm=s->param->basic.ratio_multilevel[level]
*s->target_variable.fric_rpm/MAX_FRIC_RPM; *s->target_variable.fric_rpm/MAX_FRIC_RPM;
/* 计算耦合控制权重 */ /* 计算耦合控制权重 */
float w=Shoot_CaluCoupledWeight(s,i); float w=Shoot_CaluCoupledWeight(s,i);
/* 计算跟随输出、计算修正输出 */ /* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i], s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
target_rpm, target_rpm,
s->var_fric.normalized_fil_rpm[i], s->var_fric.normalized_fil_rpm[i],
@ -422,9 +422,9 @@ int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
float target_rpm=s->param->basic.ratio_multilevel[level] float target_rpm=s->param->basic.ratio_multilevel[level]
*s->target_variable.fric_rpm/MAX_FRIC_RPM; *s->target_variable.fric_rpm/MAX_FRIC_RPM;
/* 计算耦合控制权重 */ /* 计算耦合控制权重 */
float w=Shoot_CaluCoupledWeight(s,i); float w=Shoot_CaluCoupledWeight(s,i);
/* 计算跟随输出、计算修正输出 */ /* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i], s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
target_rpm, target_rpm,
s->var_fric.normalized_fil_rpm[i], s->var_fric.normalized_fil_rpm[i],
0, 0,
@ -649,10 +649,14 @@ int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
return SHOOT_OK; return SHOOT_OK;
} }
/**
* @brief 导出射击UI数据
*
* @param s 射击结构体
* @param ui UI结构体
*/
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui) {
ui->mode = s->mode;
ui->fire = s->running_state;
}

13
User/module/shoot.h
View File

@ -44,6 +44,12 @@ typedef enum {
SHOOT_MODE_NUM SHOOT_MODE_NUM
}Shoot_Mode_t; }Shoot_Mode_t;
/* UI 导出结构（供 referee 系统绘制） */
typedef struct {
Shoot_Mode_t mode;
Shoot_Running_State_t fire;
} Referee_ShootUI_t;
typedef enum { typedef enum {
SHOOT_PROJECTILE_17MM, SHOOT_PROJECTILE_17MM,
SHOOT_PROJECTILE_42MM, SHOOT_PROJECTILE_42MM,
@ -233,6 +239,13 @@ int8_t Shoot_UpdateFeedback(Shoot_t *s);
*/ */
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd); int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
/**
* @brief 导出射击UI数据
*
* @param s 射击结构体
* @param ui UI结构体
*/
void Shoot_DumpUI(Shoot_t *s, Referee_ShootUI_t *ui);
#ifdef __cplusplus #ifdef __cplusplus

613
User/module/shoot_Master&Slave.c
View File

@ -1,613 +0,0 @@
/*
* far♂蛇模块
*/
/********************************* 使用示例 **********************************/
/*1.配置config参数以及Config_ShootInit函数参数*/
/*2.
COMP_AT9S_CMD_t shoot_ctrl_cmd_rc;
Shoot_t shoot;
Shoot_CMD_t shoot_cmd;
void Task(void *argument) {
Config_ShootInit();
Shoot_Init(&shoot,&Config_GetRobotParam()->shoot_param,SHOOT_CTRL_FREQ);
Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); 关于模式选择：初始化一个模式
while (1) {
shoot_cmd.online =shoot_ctrl_cmd_rc.online;
shoot_cmd.ready =shoot_ctrl_cmd_rc.shoot.ready;
shoot_cmd.firecmd =shoot_ctrl_cmd_rc.shoot.firecmd;
shoot.mode =shoot_ctrl_cmd_rc.mode; 关于模式选择：或者用遥控器随时切换模式，二选一
Chassis_UpdateFeedback(&shoot);
Shoot_Control(&shoot,&shoot_cmd);
}
}
*******************************************************************************/
/* Includes ----------------------------------------------------------------- */
#include <string.h>
#include "shoot_Master&Slave.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/filter/filter.h"
#include "component/user_math.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
static bool last_firecmd;
/* Private function -------------------------------------------------------- */
/**
* \brief 设置射击模式
*
* \param s 包含射击数据的结构体
* \param mode 要设置的模式
*
* \return 函数运行结果
*/
int8_t Shoot_SetMode(Shoot_t *s, Shoot_Mode_t mode)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
s->mode=mode;
s->anglecalu.num_toShoot=0;
return SHOOT_OK;
}
/**
* \brief 重置PID积分
*
* \param s 包含射击数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_ResetIntegral(Shoot_t *s)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
uint8_t fric_num = s->param->basic.fric_num;
for(int i=0;i<fric_num;i++)
{
PID_ResetIntegral(&s->pid.fric_follow[i]);
PID_ResetIntegral(&s->pid.fric_err[i]);
}
PID_ResetIntegral(&s->pid.trig);
PID_ResetIntegral(&s->pid.trig_omg);
return SHOOT_OK;
}
/**
* \brief 重置计算模块
*
* \param s 包含射击数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_ResetCalu(Shoot_t *s)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
uint8_t fric_num = s->param->basic.fric_num;
for(int i=0;i<fric_num;i++)
{
PID_Reset(&s->pid.fric_follow[i]);
PID_Reset(&s->pid.fric_err[i]);
LowPassFilter2p_Reset(&s->filter.fric.in[i], 0.0f);
LowPassFilter2p_Reset(&s->filter.fric.out[i], 0.0f);
}
PID_Reset(&s->pid.trig);
PID_Reset(&s->pid.trig_omg);
LowPassFilter2p_Reset(&s->filter.trig.in, 0.0f);
LowPassFilter2p_Reset(&s->filter.trig.out, 0.0f);
return SHOOT_OK;
}
/**
* \brief 重置输出
*
* \param s 包含射击数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_ResetOutput(Shoot_t *s)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
uint8_t fric_num = s->param->basic.fric_num;
for(int i=0;i<fric_num;i++)
{
s->output.out_follow[i]=0.0f;
s->output.out_err[i]=0.0f;
s->output.out_fric[i]=0.0f;
s->output.lpfout_fric[i]=0.0f;
}
s->output.outagl_trig=0.0f;
s->output.outomg_trig=0.0f;
s->output.outlpf_trig=0.0f;
return SHOOT_OK;
}
//float last_angle=0.0f;
//float speed=0.0f;
//int8_t Shoot_CalufeedbackRPM(Shoot_t *s)
//{
// if (s == NULL) {
// return SHOOT_ERR_NULL; // 参数错误
// }
//// static
// float err;
// err=CircleError(s->feedback.fric[0].rotor_abs_angle,last_angle,M_2PI);
// speed=err/s->dt/M_2PI*60.0f;
// last_angle=s->feedback.fric->rotor_abs_angle;
// return SHOOT_OK;
//}
/**
* \brief 根据目标弹丸速度计算摩擦轮目标转速
*
* \param s 包含射击数据的结构体
* \param target_speed 目标弹丸速度，单位m/s
*
* \return 函数运行结果
*/
int8_t Shoot_CaluTargetRPM(Shoot_t *s, float target_speed)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
switch(s->param->basic.projectileType)
{
case SHOOT_PROJECTILE_17MM:
s->target_variable.fric_rpm=5000.0f;
break;
case SHOOT_PROJECTILE_42MM:
s->target_variable.fric_rpm=4000.0f;
break;
}
return SHOOT_OK;
}
/**
* \brief 根据发射弹丸数量及发射频率计算拨弹电机目标角度
*
* \param s 包含发射数据的结构体
* \param cmd 包含射击指令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_CaluTargetAngle(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || s->anglecalu.num_toShoot == 0) {
return SHOOT_ERR_NULL;
}
float dt = s->timer.now - s->anglecalu.time_lastShoot;
float dpos;
dpos = CircleError(s->target_variable.trig_angle, s->feedback.trig_agl, M_2PI);
if(dt >= 1.0f/s->param->basic.shot_freq && cmd->firecmd && dpos<=1.0f)
{
s->anglecalu.time_lastShoot=s->timer.now;
CircleAdd(&s->target_variable.trig_angle, M_2PI/s->param->basic.num_trig_tooth, M_2PI);
s->anglecalu.num_toShoot--;
}
return SHOOT_OK;
}
/**
* \brief 更新射击模块的电机反馈信息
*
* \param s 包含射击数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_UpdateFeedback(Shoot_t *s)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
float rpm_sum=0.0f;
uint8_t fric_num = s->param->basic.fric_num;
for(int i = 0; i < fric_num; i++) {
/* 更新摩擦轮电机反馈 */
MOTOR_RM_Update(&s->param->motor.fric[i].param);
MOTOR_RM_t *motor_fed = MOTOR_RM_GetMotor(&s->param->motor.fric[i].param);
if(motor_fed!=NULL)
{
s->feedback.fric[i]=motor_fed->motor.feedback;
}
/* 滤波摩擦轮电机转速反馈 */
s->feedback.fil_fric_rpm[i] = LowPassFilter2p_Apply(&s->filter.fric.in[i], s->feedback.fric[i].rotor_speed);
/* 归一化摩擦轮电机转速反馈 */
s->feedback.fric_rpm[i] = s->feedback.fil_fric_rpm[i] / MAX_FRIC_RPM;
if(s->feedback.fric_rpm[i]>1.0f)s->feedback.fric_rpm[i]=1.0f;
if(s->feedback.fric_rpm[i]<-1.0f)s->feedback.fric_rpm[i]=-1.0f;
/* 计算平均摩擦轮电机转速反馈 */
rpm_sum+=s->feedback.fric_rpm[i];
}
s->feedback.fric_avgrpm=rpm_sum/fric_num;
/* 更新拨弹电机反馈 */
MOTOR_RM_Update(&s->param->motor.trig);
s->feedback.trig = *MOTOR_RM_GetMotor(&s->param->motor.trig);
s->feedback.trig_agl=s->param->basic.extra_deceleration_ratio*s->feedback.trig.gearbox_total_angle;
while(s->feedback.trig_agl<0)s->feedback.trig_agl+=M_2PI;
while(s->feedback.trig_agl>=M_2PI)s->feedback.trig_agl-=M_2PI;
if (s->feedback.trig.motor.reverse) {
s->feedback.trig_agl = M_2PI - s->feedback.trig_agl;
}
s->feedback.fil_trig_rpm = LowPassFilter2p_Apply(&s->filter.trig.in, s->feedback.trig.feedback.rotor_speed);
s->feedback.trig_rpm = s->feedback.trig.feedback.rotor_speed / MAX_TRIG_RPM;
if(s->feedback.trig_rpm>1.0f)s->feedback.trig_rpm=1.0f;
if(s->feedback.trig_rpm<-1.0f)s->feedback.trig_rpm=-1.0f;
s->errtosee = s->feedback.fric[0].rotor_speed - s->feedback.fric[1].rotor_speed;
return SHOOT_OK;
}
/**
* \brief 射击模块运行状态机
*
* \param s 包含射击数据的结构体
* \param cmd 包含射击指令的结构体
*
* \return 函数运行结果
*/float a;
int8_t Shoot_RunningFSM(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || cmd == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
uint8_t fric_num = s->param->basic.fric_num;
if(!s->online /*|| s->mode==SHOOT_MODE_SAFE*/){
for(int i=0;i<fric_num;i++)
{
MOTOR_RM_Relax(&s->param->motor.fric[i].param);
}
MOTOR_RM_Relax(&s->param->motor.trig);
}
else{
static float pos;
switch(s->running_state)
{
case SHOOT_STATE_IDLE:/*熄火等待*/
for(int i=0;i<fric_num;i++)
{ /* 转速归零 */
PID_ResetIntegral(&s->pid.fric_follow[i]);
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],0.0f,s->feedback.fric_rpm[i],0,s->timer.dt);
s->output.out_fric[i]=s->output.out_follow[i];
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
}
s->output.outagl_trig =PID_Calc(&s->pid.trig,pos,s->feedback.trig_agl,0,s->timer.dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,s->output.outagl_trig,s->feedback.trig_rpm,0,s->timer.dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
/* 检查状态机 */
if(cmd->ready)
{
Shoot_ResetCalu(s);
Shoot_ResetIntegral(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_READY;
}
break;
case SHOOT_STATE_READY:/*准备射击*/
for(int i=0;i<fric_num;i++)
{
uint8_t level=s->param->motor.fric->level-1;
float fric_rpm=s->param->basic.ratio_multilevel[level]*s->target_variable.fric_rpm/MAX_FRIC_RPM;
/* 计算跟随输出、计算修正输出 */
a=s->target_variable.fric_rpm/MAX_FRIC_RPM;
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
fric_rpm,
s->feedback.fric_rpm[i],
0,
s->timer.dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],
s->feedback.fric_avgrpm,
s->feedback.fric_rpm[i],
0,
s->timer.dt);
/* 按比例缩放并加和输出 */
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
/* 滤波 */
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
/* 设置输出 */
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
}
/* 设置拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig,
pos,
s->feedback.trig_agl,
0,
s->timer.dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
s->output.outagl_trig,
s->feedback.trig_rpm,
0,
s->timer.dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
/* 检查状态机 */
if(!cmd->ready)
{
Shoot_ResetCalu(s);
Shoot_ResetOutput(s);
s->running_state=SHOOT_STATE_IDLE;
}
else if(last_firecmd==false&&cmd->firecmd==true)
{
s->running_state=SHOOT_STATE_FIRE;
/* 根据模式设置待发射弹数 */
switch(s->mode)
{
case SHOOT_MODE_SINGLE:
s->anglecalu.num_toShoot=1;
break;
case SHOOT_MODE_BURST:
s->anglecalu.num_toShoot=s->param->basic.shot_burst_num;
break;
case SHOOT_MODE_CONTINUE:
s->anglecalu.num_toShoot=6666;
break;
default:
s->anglecalu.num_toShoot=0;
break;
}
}
break;
case SHOOT_STATE_FIRE:/*射击*/
Shoot_CaluTargetAngle(s, cmd);
for(int i=0;i<fric_num;i++)
{
uint8_t level=s->param->motor.fric->level-1;
float fric_rpm=s->param->basic.ratio_multilevel[level]*s->target_variable.fric_rpm/MAX_FRIC_RPM;
/* 计算跟随输出、计算修正输出 */
s->output.out_follow[i]=PID_Calc(&s->pid.fric_follow[i],
fric_rpm,
s->feedback.fric_rpm[i],
0,
s->timer.dt);
s->output.out_err[i]=PID_Calc(&s->pid.fric_err[i],
s->feedback.fric_avgrpm,
s->feedback.fric_rpm[i],
0,
s->timer.dt);
/* 按比例缩放并加和输出 */
ScaleSumTo1(&s->output.out_follow[i], &s->output.out_err[i]);
s->output.out_fric[i]=s->output.out_follow[i]+s->output.out_err[i];
/* 滤波 */
s->output.lpfout_fric[i] = LowPassFilter2p_Apply(&s->filter.fric.out[i], s->output.out_fric[i]);
/* 设置输出 */
MOTOR_RM_SetOutput(&s->param->motor.fric[i].param, s->output.lpfout_fric[i]);
}
/* 设置拨弹电机输出 */
s->output.outagl_trig =PID_Calc(&s->pid.trig,
s->target_variable.trig_angle,
s->feedback.trig_agl,
0,
s->timer.dt);
s->output.outomg_trig =PID_Calc(&s->pid.trig_omg,
s->output.outagl_trig,
s->feedback.trig_rpm,
0,
s->timer.dt);
s->output.outlpf_trig =LowPassFilter2p_Apply(&s->filter.trig.out, s->output.outomg_trig);
MOTOR_RM_SetOutput(&s->param->motor.trig, s->output.outlpf_trig);
/* 检查状态机 */
if(!cmd->firecmd)
{
s->running_state=SHOOT_STATE_READY;
pos=s->feedback.trig_agl;
}
break;
default:
s->running_state=SHOOT_STATE_IDLE;
break;
}
}
/* 输出 */
MOTOR_RM_Ctrl(&s->param->motor.fric[0].param);
if(s->param->basic.fric_num>4)
{
MOTOR_RM_Ctrl(&s->param->motor.fric[4].param);
}
MOTOR_RM_Ctrl(&s->param->motor.trig);
last_firecmd = cmd->firecmd;
return SHOOT_OK;
}
/**
* \brief 射击模块堵塞检测状态机
*
* \param s 包含射击数据的结构体
* \param cmd 包含射击指令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_JamDetectionFSM(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
if(s->param->jamDetection.enable){
switch (s->jamdetection.fsmState) {
case SHOOT_JAMFSM_STATE_NORMAL:/* 正常运行 */
/* 检测电流是否超过阈值 */
if (s->feedback.trig.feedback.torque_current/1000.0f > s->param->jamDetection.threshold) {
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_SUSPECTED;
s->jamdetection.lastTime = s->timer.now; /* 记录怀疑开始时间 */
}
/* 正常运行射击状态机 */
Shoot_RunningFSM(s, cmd);
break;
case SHOOT_JAMFSM_STATE_SUSPECTED:/* 怀疑堵塞 */
/* 检测电流是否低于阈值 */
if (s->feedback.trig.feedback.torque_current/1000.0f < s->param->jamDetection.threshold) {
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
break;
}
/* 检测高阈值状态是否超过设定怀疑时间 */
else if ((s->timer.now - s->jamdetection.lastTime) >= s->param->jamDetection.suspectedTime) {
s->jamdetection.detected =true;
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_CONFIRMED;
break;
}
/* 正常运行射击状态机 */
Shoot_RunningFSM(s, cmd);
break;
case SHOOT_JAMFSM_STATE_CONFIRMED:/* 确认堵塞 */
/* 清空待发射弹 */
s->anglecalu.num_toShoot=0;
/* 修改拨弹盘目标角度 */
s->target_variable.trig_angle = s->feedback.trig_agl-(M_2PI/s->param->basic.num_trig_tooth);
/* 切换状态 */
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_DEAL;
/* 记录处理开始时间 */
s->jamdetection.lastTime = s->timer.now;
case SHOOT_JAMFSM_STATE_DEAL:/* 堵塞处理 */
/* 正常运行射击状态机 */
Shoot_RunningFSM(s, cmd);
/* 给予0.3秒响应时间并检测电流小于20A，认为堵塞已解除 */
if ((s->timer.now - s->jamdetection.lastTime)>=0.3f&&s->feedback.trig.feedback.torque_current/1000.0f < 20.0f) {
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
}
break;
default:
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
break;
}
}
else{
s->jamdetection.fsmState = SHOOT_JAMFSM_STATE_NORMAL;
s->jamdetection.detected = false;
Shoot_RunningFSM(s, cmd);
}
return SHOOT_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* \brief 初始化射击模块
*
* \param s 包含射击数据的结构体
* \param param 包含射击参数的结构体
* \param target_freq 控制循环频率，单位Hz
*
* \return 函数运行结果
*/
int8_t Shoot_Init(Shoot_t *s, Shoot_Params_t *param, float target_freq)
{
if (s == NULL || param == NULL || target_freq <= 0.0f) {
return SHOOT_ERR_NULL; // 参数错误
}
uint8_t fric_num = param->basic.fric_num;
s->param=param;
BSP_FDCAN_Init();
/* 初始化摩擦轮PID和滤波器 */
for(int i=0;i<fric_num;i++){
MOTOR_RM_Register(&param->motor.fric[i].param);
PID_Init(&s->pid.fric_follow[i],
KPID_MODE_CALC_D,
target_freq,
&param->pid.fric_follow);
PID_Init(&s->pid.fric_err[i],
KPID_MODE_CALC_D,
target_freq,
&param->pid.fric_err);
LowPassFilter2p_Init(&s->filter.fric.in[i],
target_freq,
s->param->filter.fric.in);
LowPassFilter2p_Init(&s->filter.fric.out[i],
target_freq,
s->param->filter.fric.out);
}
/* 初始化拨弹PID和滤波器 */
MOTOR_RM_Register(&param->motor.trig);
switch(s->param->motor.trig.module)
{
case MOTOR_M3508:
PID_Init(&s->pid.trig,
KPID_MODE_CALC_D,
target_freq,
&param->pid.trig_3508);
PID_Init(&s->pid.trig_omg,
KPID_MODE_CALC_D,
target_freq,
&param->pid.trig_omg_3508);
break;
case MOTOR_M2006:
PID_Init(&s->pid.trig,
KPID_MODE_CALC_D,
target_freq,
&param->pid.trig_2006);
PID_Init(&s->pid.trig_omg,
KPID_MODE_CALC_D,
target_freq,
&param->pid.trig_omg_2006);
break;
default:
return SHOOT_ERR_MOTOR;
break;
}
LowPassFilter2p_Init(&s->filter.trig.in,
target_freq,
s->param->filter.trig.in);
LowPassFilter2p_Init(&s->filter.trig.out,
target_freq,
s->param->filter.trig.out);
/* 归零变量 */
memset(&s->anglecalu,0,sizeof(s->anglecalu));
return SHOOT_OK;
}
/**
* \brief 射击模块控制主函数
*
* \param s 包含射击数据的结构体
* \param cmd 包含射击指令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd)
{
if (s == NULL || cmd == NULL) {
return SHOOT_ERR_NULL; // 参数错误
}
s->timer.now = BSP_TIME_Get_us() / 1000000.0f;
s->timer.dt = (BSP_TIME_Get_us() - s->timer.lask_wakeup) / 1000000.0f;
s->timer.lask_wakeup = BSP_TIME_Get_us();
s->online = cmd->online;
// Shoot_CaluTargetRPM(s,233);
Shoot_JamDetectionFSM(s, cmd);
// Shoot_CalufeedbackRPM(s);
return SHOOT_OK;
}

243
User/module/shoot_Master&Slave.h
View File

@ -1,243 +0,0 @@
/*
* far♂蛇模块
*/
#pragma once
#include <stddef.h>
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h"
#include "component/pid.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#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) /* 内存分配失败 */
#define MAX_FRIC_RPM 7000.0f
#define MAX_TRIG_RPM 5000.0f//这里可能也会影响最高发射频率，待测试
#define MAX_FRIC_NUM 6
#define MAX_NUM_MULTILEVEL 2 /* 多级发射级数 */
/* 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_Running_State_t;
typedef enum {
SHOOT_MODE_SAFE = 0,/* 安全模式 */
SHOOT_MODE_SINGLE, /* 单发模式 */
SHOOT_MODE_BURST, /* 多发模式 */
SHOOT_MODE_CONTINUE,/* 连发模式 */
SHOOT_MODE_NUM
}Shoot_Mode_t;
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;
typedef struct {
MOTOR_Feedback_t fric[MAX_FRIC_NUM];/* 摩擦轮电机反馈 */
MOTOR_RM_t trig; /* 拨弹电机反馈 */
float trig_agl; /*计算所有减速比后的拨弹盘的角度*/
float fil_fric_rpm[MAX_FRIC_NUM]; /* 滤波后的摩擦轮转速 */
float fil_trig_rpm; /* 滤波后的拨弹电机转速*/
float fric_rpm[MAX_FRIC_NUM]; /* 归一化摩擦轮转速 */
float fric_avgrpm; /* 归一化摩擦轮平均转速 */
float trig_rpm; /* 归一化拨弹电机转速 */
}Shoot_Feedback_t;
typedef struct{
float time_lastShoot;/* 上次射击时间 */
uint16_t num_toShoot;/* 剩余待发射弹数 */
uint16_t num_shooted;/* 已发射弹数 */
}Shoot_trigAngleCalu_t;
typedef struct {
bool detected; /* 卡弹检测结果 */
float lastTime;/* 用于记录怀疑状态或处理状态的开始时间 */
Shoot_JamDetectionFSM_State_t fsmState; /* 卡弹检测状态机 */
}Shoot_JamDetection_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 {
bool online; /* 在线标志 */
Shoot_Mode_t mode;/* 射击模式 */
bool ready; /* 准备射击 */
bool firecmd; /* 射击 */
}Shoot_CMD_t;
/* 底盘参数的结构体，包含所有初始化用的参数，通常是const，存好几组 */
typedef struct {
struct{
Shoot_Projectile_t projectileType; /* 发射弹丸类型 */;
size_t fric_num; /* 摩擦轮电机数量 */
size_t num_multilevel; /* 多级发射级数 */
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 {
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_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; /* 输出值滤波器截止频率 */
}fric;
struct{
float in; /* 反馈值滤波器截止频率 */
float out; /* 输出值滤波器截止频率 */
}trig;
} filter;/* 滤波器截止频率参数 */
} Shoot_Params_t;
typedef struct {
float now; /* 当前时间，单位秒 */
uint64_t lask_wakeup; /* 上次唤醒时间，单位微秒 */
float dt; /* 两次唤醒间隔时间，单位秒 */
}Shoot_Timer_t;
/*
* 运行的主结构体，所有这个文件里的函数都在操作这个结构体
* 包含了初始化参数，中间变量，输出变量
*/
typedef struct {
bool online; /*在线检测*/
Shoot_Timer_t timer; /* 计时器 */
Shoot_Params_t *param; /* 发射参数 */
/* 模块通用 */
Shoot_Mode_t mode; /* 射击模式 */
/* 反馈信息 */
Shoot_Feedback_t feedback;
/* 控制信息*/
Shoot_Running_State_t running_state; /* 运行状态机 */
Shoot_JamDetection_t jamdetection; /* 卡弹检测控制信息 */
Shoot_trigAngleCalu_t anglecalu; /* 角度计算控制信息 */
Shoot_Output_t output; /* 输出信息 */
/* 目标控制量 */
struct {
float fric_rpm; /* 目标摩擦轮转速 */
float trig_angle;/* 目标拨弹位置 */
}target_variable;
/* 反馈控制用的PID */
struct {
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[MAX_FRIC_NUM]; /* 反馈值滤波器 */
LowPassFilter2p_t out[MAX_FRIC_NUM];/* 输出值滤波器 */
}fric;
struct{
LowPassFilter2p_t in; /* 反馈值滤波器 */
LowPassFilter2p_t out;/* 输出值滤波器 */
}trig;
} filter;
float errtosee; /*调试用*/
} Shoot_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief 初始化发射
*
* \param s 包含发射数据的结构体
* \param param 包含发射参数的结构体指针
* \param target_freq 任务预期的运行频率
*
* \return 函数运行结果
*/
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 更新反馈
*
* \param s 包含发射数据的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_UpdateFeedback(Shoot_t *s);
/**
* \brief 初始化发射
*
* \param s 包含发射数据的结构体
* \param cmd 包含发射命令的结构体
*
* \return 函数运行结果
*/
int8_t Shoot_Control(Shoot_t *s, Shoot_CMD_t *cmd);
#ifdef __cplusplus
}
#endif

31
User/task/cmd.c
View File

@ -31,10 +31,15 @@ AT9S_t cmd_at9s;
#endif #endif
AI_cmd_t cmd_ai; AI_cmd_t cmd_ai;
#if CMD_ENABLE_SRC_REF
CMD_RawInput_REF_t cmd_ref;
#endif
Shoot_CMD_t *cmd_for_shoot; Shoot_CMD_t *cmd_for_shoot;
Chassis_CMD_t *cmd_for_chassis; Chassis_CMD_t *cmd_for_chassis;
Gimbal_CMD_t *cmd_for_gimbal; Gimbal_CMD_t *cmd_for_gimbal;
Track_CMD_t *cmd_for_track; Track_CMD_t *cmd_for_track;
Referee_UI_CMD_t *cmd_for_ui;
static CMD_t cmd; static CMD_t cmd;
AI_t ai; AI_t ai;
@ -70,6 +75,14 @@ void Task_cmd(void *argument) {
/* 从CAN2接收AI命令 */ /* 从CAN2接收AI命令 */
AI_ParseCmdFromCan( &ai,&cmd_ai); AI_ParseCmdFromCan( &ai,&cmd_ai);
#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); CMD_Update(&cmd);
/* 获取命令发送到各模块 */ /* 获取命令发送到各模块 */
@ -77,7 +90,7 @@ void Task_cmd(void *argument) {
cmd_for_gimbal = CMD_GetGimbalCmd(&cmd); cmd_for_gimbal = CMD_GetGimbalCmd(&cmd);
cmd_for_shoot = CMD_GetShootCmd(&cmd); cmd_for_shoot = CMD_GetShootCmd(&cmd);
cmd_for_track = CMD_GetTrackCmd(&cmd); cmd_for_track = CMD_GetTrackCmd(&cmd);
cmd_for_ui = CMD_GetRefUICmd(&cmd);
if (cmd_for_shoot->firecmd) { if (cmd_for_shoot->firecmd) {
/* 发射时添加pitch前馈，抵消后坐力。此时为负值表示向下压。需要根据实际情况调整 */ /* 发射时添加pitch前馈，抵消后坐力。此时为负值表示向下压。需要根据实际情况调整 */
cmd_for_gimbal->feedforward_pit = -1.0f; cmd_for_gimbal->feedforward_pit = -1.0f;
@ -93,6 +106,22 @@ void Task_cmd(void *argument) {
osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0); osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0);
osMessageQueueReset(task_runtime.msgq.track.cmd); osMessageQueueReset(task_runtime.msgq.track.cmd);
osMessageQueuePut(task_runtime.msgq.track.cmd, cmd_for_track, 0, 0); osMessageQueuePut(task_runtime.msgq.track.cmd, cmd_for_track, 0, 0);
#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
osMessageQueueReset(task_runtime.msgq.referee.ui.frcmd);
osMessageQueuePut(task_runtime.msgq.referee.ui.frcmd, cmd_for_ui, 0, 0);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

26
User/task/init.c
View File

@ -13,6 +13,7 @@
#include "module/shoot.h" #include "module/shoot.h"
#include "module/chassis.h" #include "module/chassis.h"
#include "module/track.h" #include "module/track.h"
#include "device/referee.h"
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -44,25 +45,48 @@ void Task_Init(void *argument) {
task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd); task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
task_runtime.thread.supercap = osThreadNew(Task_supercap, NULL, &attr_supercap); task_runtime.thread.supercap = osThreadNew(Task_supercap, NULL, &attr_supercap);
// task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); // task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
task_runtime.thread.referee = osThreadNew(Task_referee, NULL, &attr_referee);
// 创建消息队列 // 创建消息队列
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
/* cmd */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
#if CMD_RCTypeTable_Index == 0 #if CMD_RCTypeTable_Index == 0
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL);
#elif CMD_RCTypeTable_Index == 1 #elif CMD_RCTypeTable_Index == 1
task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL); task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL);
#endif #endif
/* 底盘 */
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(float), NULL); task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(float), NULL);
task_runtime.msgq.chassis.cmd= osMessageQueueNew(3u, sizeof(Chassis_CMD_t), NULL); task_runtime.msgq.chassis.cmd= osMessageQueueNew(3u, sizeof(Chassis_CMD_t), NULL);
/* 云台 */
task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL); task_runtime.msgq.gimbal.imu= osMessageQueueNew(2u, sizeof(Gimbal_IMU_t), NULL);
task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL); task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
/* 发射 */
task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL); task_runtime.msgq.shoot.cmd = osMessageQueueNew(2u, sizeof(Shoot_CMD_t), NULL);
/* 履带 */
task_runtime.msgq.track.cmd = osMessageQueueNew(2u, sizeof(Track_CMD_t), NULL); task_runtime.msgq.track.cmd = osMessageQueueNew(2u, sizeof(Track_CMD_t), NULL);
/* AI */
task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL); task_runtime.msgq.ai.rawdata_FromGimbal = osMessageQueueNew(2u, sizeof(Gimbal_Feedback_t), NULL);
task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); task_runtime.msgq.ai.rawdata_FromIMU = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
// task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(), NULL); // task_runtime.msgq.ai.rawdata_FromShoot = osMessageQueueNew(2u, sizeof(), 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(Referee_ChassisUI_t), NULL);
task_runtime.msgq.referee.ui.tocap =osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
task_runtime.msgq.referee.ui.togimbal =osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
task_runtime.msgq.referee.ui.toshoot =osMessageQueueNew(2u, sizeof(Referee_ShootUI_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 */ /* USER MESSAGE END */
osKernelUnlock(); // 解锁内核 osKernelUnlock(); // 解锁内核

97
User/task/referee.c Normal file
View File

@ -0,0 +1,97 @@
/*
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 > 500)
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) {
Referee_PraseCmd(&ui, ref_cmd);
// Referee_StartSend(send_data, sizeof(send_data));
}
Referee_PackUI(&ui, &ref);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

5
User/task/user_task.c
View File

@ -54,3 +54,8 @@ const osThreadAttr_t attr_ai = {
.priority = osPriorityNormal, .priority = osPriorityNormal,
.stack_size = 256 * 4, .stack_size = 256 * 4,
}; };
const osThreadAttr_t attr_referee = {
.name = "referee",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

36
User/task/user_task.h
View File

@ -22,6 +22,7 @@ extern "C" {
#define CMD_FREQ (500.0) #define CMD_FREQ (500.0)
#define SUPERCAP_FREQ (500.0) #define SUPERCAP_FREQ (500.0)
#define AI_FREQ (500.0) #define AI_FREQ (500.0)
#define REFEREE_FREQ (500.0)
/* 任务初始化延时ms */ /* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u) #define TASK_INIT_DELAY (100u)
#define RC_INIT_DELAY (0) #define RC_INIT_DELAY (0)
@ -33,6 +34,7 @@ extern "C" {
#define CMD_INIT_DELAY (0) #define CMD_INIT_DELAY (0)
#define SUPERCAP_INIT_DELAY (0) #define SUPERCAP_INIT_DELAY (0)
#define AI_INIT_DELAY (0) #define AI_INIT_DELAY (0)
#define REFEREE_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */ /* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
@ -49,7 +51,8 @@ typedef struct {
osThreadId_t ctrl_shoot; osThreadId_t ctrl_shoot;
osThreadId_t cmd; osThreadId_t cmd;
osThreadId_t supercap; osThreadId_t supercap;
osThreadId_t ai; osThreadId_t ai;
osThreadId_t referee;
} thread; } thread;
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
@ -60,6 +63,7 @@ typedef struct {
osMessageQueueId_t imu; osMessageQueueId_t imu;
osMessageQueueId_t cmd; osMessageQueueId_t cmd;
osMessageQueueId_t yaw; osMessageQueueId_t yaw;
osMessageQueueId_t ref; /* Referee_ForChassis_t, cmd转发 */
}chassis; }chassis;
struct { struct {
osMessageQueueId_t imu; osMessageQueueId_t imu;
@ -67,12 +71,13 @@ typedef struct {
}gimbal; }gimbal;
struct{ struct{
osMessageQueueId_t cmd; osMessageQueueId_t cmd;
osMessageQueueId_t ref; /* Referee_ForShoot_t, cmd转发 */
}shoot; }shoot;
struct{ struct{
osMessageQueueId_t cmd; osMessageQueueId_t cmd;
}track; }track;
struct{ struct{
osMessageQueueId_t rc; osMessageQueueId_t rc;
osMessageQueueId_t pc; osMessageQueueId_t pc;
osMessageQueueId_t nuc; osMessageQueueId_t nuc;
osMessageQueueId_t ref; osMessageQueueId_t ref;
@ -81,7 +86,27 @@ typedef struct {
osMessageQueueId_t rawdata_FromGimbal; osMessageQueueId_t rawdata_FromGimbal;
osMessageQueueId_t rawdata_FromShoot; osMessageQueueId_t rawdata_FromShoot;
osMessageQueueId_t rawdata_FromIMU; osMessageQueueId_t rawdata_FromIMU;
osMessageQueueId_t ref; /* Referee_ForAI_t, cmd转发 */
}ai; }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; } msgq;
/* USER MESSAGE END */ /* USER MESSAGE END */
@ -107,6 +132,7 @@ typedef struct {
UBaseType_t cmd; UBaseType_t cmd;
UBaseType_t supercap; UBaseType_t supercap;
UBaseType_t ai; UBaseType_t ai;
UBaseType_t referee;
} stack_water_mark; } stack_water_mark;
/* 各任务运行频率 */ /* 各任务运行频率 */
@ -120,6 +146,7 @@ typedef struct {
float cmd; float cmd;
float supercap; float supercap;
float ai; float ai;
float referee;
} freq; } freq;
/* 任务最近运行时间 */ /* 任务最近运行时间 */
@ -133,6 +160,7 @@ typedef struct {
float cmd; float cmd;
float supercap; float supercap;
float ai; float ai;
float referee;
} last_up_time; } last_up_time;
} Task_Runtime_t; } Task_Runtime_t;
@ -151,6 +179,7 @@ extern const osThreadAttr_t attr_ctrl_shoot;
extern const osThreadAttr_t attr_cmd; extern const osThreadAttr_t attr_cmd;
extern const osThreadAttr_t attr_supercap; extern const osThreadAttr_t attr_supercap;
extern const osThreadAttr_t attr_ai; extern const osThreadAttr_t attr_ai;
extern const osThreadAttr_t attr_referee;
/* 任务函数声明 */ /* 任务函数声明 */
void Task_Init(void *argument); void Task_Init(void *argument);
void Task_rc(void *argument); void Task_rc(void *argument);
@ -162,6 +191,7 @@ void Task_ctrl_shoot(void *argument);
void Task_cmd(void *argument); void Task_cmd(void *argument);
void Task_supercap(void *argument); void Task_supercap(void *argument);
void Task_ai(void *argument); void Task_ai(void *argument);
void Task_referee(void *argument);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif