233 lines
11 KiB
C
233 lines
11 KiB
C
/**
|
||
* @file referee.C
|
||
* @author kidneygood (you@domain.com)
|
||
* @brief
|
||
* @version 0.1
|
||
* @date 2022-11-18
|
||
*
|
||
* @copyright Copyright (c) 2022
|
||
*
|
||
*/
|
||
#include "referee_task.h"
|
||
#include "robot_def.h"
|
||
#include "rm_referee.h"
|
||
#include "referee_UI.h"
|
||
#include "string.h"
|
||
#include "cmsis_os.h"
|
||
#include "chassis.h"
|
||
#include "super_cap.h"
|
||
|
||
extern SuperCapInstance *cap; // 超级电容
|
||
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
|
||
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
|
||
Referee_Interactive_info_t ui_data;
|
||
uint8_t UI_Seq; // 包序号，供整个referee文件使用
|
||
// @todo 不应该使用全局变量
|
||
|
||
/**
|
||
* @brief 判断各种ID，选择客户端ID
|
||
* @param referee_info_t *referee_recv_info
|
||
* @retval none
|
||
* @attention
|
||
*/
|
||
static void DeterminRobotID()
|
||
{
|
||
// id小于7是红色,大于7是蓝色 #define Robot_Red 0 #define Robot_Blue 1
|
||
referee_recv_info->referee_id.Robot_Color = referee_recv_info->GameRobotState.robot_id > 7 ? Robot_Blue : Robot_Red;
|
||
referee_recv_info->referee_id.Robot_ID = referee_recv_info->GameRobotState.robot_id;
|
||
referee_recv_info->referee_id.Cilent_ID = 0x0100 + referee_recv_info->referee_id.Robot_ID; // 计算客户端ID
|
||
referee_recv_info->referee_id.Receiver_Robot_ID = 0;
|
||
}
|
||
|
||
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
|
||
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
|
||
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
|
||
static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data);
|
||
|
||
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
|
||
{
|
||
referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
|
||
Interactive_data = UI_data; // 获取UI绘制需要的机器人状态数据
|
||
referee_recv_info->init_flag = 1;
|
||
return referee_recv_info;
|
||
}
|
||
|
||
void UITask()
|
||
{
|
||
MyUIRefresh(referee_recv_info, Interactive_data);
|
||
xyz_refresh(referee_recv_info, Interactive_data);
|
||
}
|
||
|
||
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次
|
||
static String_Data_t UI_State_dyn[7]; // 机器人状态,动态先add才能change
|
||
static Graph_Data_t UI_State_dyn_graph[2];
|
||
static Graph_Data_t UI_angle[4];
|
||
|
||
#define AXIS_LEN 250
|
||
#define AXIS_X0 550
|
||
#define AXIS_Y0 80
|
||
static Graph_Data_t xyz_axis[6]; // 抬升 横移位置
|
||
|
||
void MyUIInit()
|
||
{
|
||
//if (!referee_recv_info->init_flag)
|
||
//vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
|
||
while (referee_recv_info->GameRobotState.robot_id == 0)
|
||
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
|
||
|
||
DeterminRobotID(); // 确定ui要发送到的目标客户端
|
||
UIDelete(&referee_recv_info->referee_id, UI_Data_Del_ALL, 0); // 清空UI
|
||
|
||
// 绘制车辆状态标志指示
|
||
UICharDraw(&UI_State_sta[0], "ss0", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 750, "G.AIR_PUMP:");
|
||
UICharDraw(&UI_State_sta[1], "ss1", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 700, "ARM_MODE:");
|
||
|
||
UICharDraw(&UI_State_sta[2], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 650, "PITCH:");
|
||
UICharDraw(&UI_State_sta[3], "ss2", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 10, 600, "BIG_PITCH:");
|
||
|
||
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[0]);
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[1]);
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[2]);
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[3]);
|
||
|
||
// xyz坐标系 静态
|
||
UILineDraw(&xyz_axis[0], "x_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
|
||
UILineDraw(&xyz_axis[1], "y_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0+AXIS_LEN);
|
||
UILineDraw(&xyz_axis[2], "z_axis_s", UI_Graph_ADD, 7, UI_Color_Green, 1, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[0]);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[1],xyz_axis[2]);
|
||
|
||
// xyz坐标系 动态
|
||
UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0);
|
||
UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN*0.5 , AXIS_Y0+AXIS_LEN*0.5);
|
||
UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_ADD, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+AXIS_LEN*0.5);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[3]);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[4],xyz_axis[5]);
|
||
|
||
|
||
// 绘制车辆状态标志，动态
|
||
// 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新
|
||
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_ADD, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
|
||
|
||
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_ADD, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
|
||
|
||
// 关节角度显示,动态
|
||
UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
|
||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[0]);
|
||
UIFloatDraw(&UI_angle[1], "big_pitch_angle", UI_Graph_ADD, 8, UI_Color_Green, 20, 2, 2, 230, 600, Interactive_data->big_pitch_angle * 1000);
|
||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_angle[1]);
|
||
|
||
}
|
||
|
||
static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
|
||
{
|
||
UIChangeCheck(_Interactive_data);
|
||
// 显示 pitch 轴编码器位置
|
||
UIFloatDraw(&UI_angle[0], "pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 650, Interactive_data->diff_pitch_angle * 1000);
|
||
UIFloatDraw(&UI_angle[1], "big_pitch_angle", UI_Graph_Change, 8, UI_Color_Green, 20, 2, 2, 230, 600, Interactive_data->big_pitch_angle * 1000);
|
||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_angle[0],UI_angle[1]);
|
||
// gimbal
|
||
if (_Interactive_data->Referee_Interactive_Flag.gimbal_flag == 1)
|
||
{
|
||
switch (_Interactive_data->gimbal_mode)
|
||
{
|
||
case GIMBAL_ZERO_FORCE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "ZERO_FORCE");
|
||
break;
|
||
}
|
||
case GIMBAL_MANUAL_MODE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "MANUAL ");
|
||
break;
|
||
}
|
||
case GIMBAL_SILVER_MODE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "SILVER ");
|
||
break;
|
||
}
|
||
case GIMBAL_FLOOR_MODE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "FLOOR ");
|
||
break;
|
||
}
|
||
case GIMBAL_IKINE_MODE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "CUSTOM ");
|
||
break;
|
||
}
|
||
case GIMBAL_STORAGE_MODE:
|
||
{
|
||
UICharDraw(&UI_State_dyn[1], "gimbal_mode", UI_Graph_Change, 8, UI_Color_Yellow, 20, 2, 230, 700, "STORAGE ");
|
||
break;
|
||
}
|
||
}
|
||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]);
|
||
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 0;
|
||
}
|
||
// pump
|
||
if (_Interactive_data->Referee_Interactive_Flag.pump_flag == 1)
|
||
{
|
||
switch (_Interactive_data->pump_mode) {
|
||
case PUMP_STOP:
|
||
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 330, 695, 377, 743);
|
||
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Purplish_red, 10, 350, 800, 10);
|
||
break;
|
||
case PUMP_OPEN:
|
||
//UILineDraw(&UI_State_dyn_graph[0], "pump_close", UI_Graph_Change, 8, UI_Color_Purplish_red, 0, 330, 695, 330, 695);
|
||
UICircleDraw(&UI_State_dyn_graph[1], "pump_open", UI_Graph_Change, 8, UI_Color_Green, 10, 350, 800, 25);
|
||
break;
|
||
}
|
||
UIGraphRefresh(&referee_recv_info->referee_id,1,UI_State_dyn_graph[1]);
|
||
|
||
_Interactive_data->Referee_Interactive_Flag.pump_flag = 0;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 模式切换检测,模式发生切换时，对flag置位
|
||
* @param Referee_Interactive_info_t *_Interactive_data
|
||
* @retval none
|
||
* @attention
|
||
*/
|
||
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
|
||
{
|
||
if (_Interactive_data->chassis_mode != _Interactive_data->chassis_last_mode)
|
||
{
|
||
_Interactive_data->Referee_Interactive_Flag.chassis_flag = 1;
|
||
_Interactive_data->chassis_last_mode = _Interactive_data->chassis_mode;
|
||
}
|
||
|
||
if (_Interactive_data->gimbal_mode != _Interactive_data->gimbal_last_mode)
|
||
{
|
||
_Interactive_data->Referee_Interactive_Flag.gimbal_flag = 1;
|
||
_Interactive_data->gimbal_last_mode = _Interactive_data->gimbal_mode;
|
||
}
|
||
|
||
if (_Interactive_data->pump_mode != _Interactive_data->pump_last_mode)
|
||
{
|
||
_Interactive_data->Referee_Interactive_Flag.pump_flag = 1;
|
||
_Interactive_data->pump_last_mode = _Interactive_data->pump_mode;
|
||
}
|
||
}
|
||
|
||
static void xyz_refresh(referee_info_t *referee_recv_info, Referee_Interactive_info_t *_Interactive_data)
|
||
{
|
||
const float y_max = FB_MAX;//单位mm
|
||
const float z_max = UD_MAX;//单位mm
|
||
|
||
float y_ratio = _Interactive_data->xyz[1]/y_max;
|
||
float z_ratio = _Interactive_data->xyz[2]/z_max;
|
||
// xyz坐标系 动态
|
||
UILineDraw(&xyz_axis[3], "x_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+AXIS_LEN , AXIS_Y0);
|
||
UILineDraw(&xyz_axis[4], "y_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0+(AXIS_LEN*y_ratio) , AXIS_Y0+(AXIS_LEN*y_ratio));
|
||
UILineDraw(&xyz_axis[5], "z_axis_d", UI_Graph_Change, 8, UI_Color_Green, 5, AXIS_X0, AXIS_Y0,AXIS_X0 , AXIS_Y0+(AXIS_LEN*z_ratio));
|
||
UIGraphRefresh(&referee_recv_info->referee_id,2,xyz_axis[3],xyz_axis[4]);
|
||
UIGraphRefresh(&referee_recv_info->referee_id,1,xyz_axis[5]);
|
||
|
||
}
|