HFUT_Firmament_EC
/
basic_framework
12
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

联盟赛版本

This commit is contained in:
zcj 2024-04-03 01:12:50 +08:00
parent 4871c573fb
commit b0efcd64ff
13 changed files with 209 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
application/chassis/chassis.c
View File

@ -23,6 +23,7 @@
#include "referee_UI.h"
#include "arm_math.h"
#include "power_meter/power_meter.h"
#include "user_lib.h"
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
@ -50,7 +51,10 @@ static SuperCapInstance *cap; // 超级电
static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static PowerMeterInstance *power_meter;
float chassis_power = 0;
static first_order_filter_type_t vx_filter;
static first_order_filter_type_t vy_filter;
const static float32_t chassis_x_order_filter=0.05f;
const static float32_t chassis_y_order_filter=0.05f;
/* 用于自旋变速策略的时间变量 */
// static float t;
@ -119,7 +123,8 @@ void ChassisInit()
.rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}};
cap = SuperCapInit(&cap_conf); // 超级电容初始化
first_order_filter_init(&vy_filter,0.002f,&chassis_y_order_filter);
first_order_filter_init(&vx_filter,0.002f,&chassis_x_order_filter);
// PowerMeter_Init_Config_s power_conf = {
// .can_config = {
// .can_handle = &hcan2,
@ -189,7 +194,7 @@ static void LimitChassisOutput()
// motor_rb->motor_controller.motor_power_predict +
// motor_lb->motor_controller.motor_power_predict +
// motor_lf->motor_controller.motor_power_predict + 3.6f;
float P_max = 55;
float P_max = chassis_cmd_recv.chassis_power_limit ;
float K = P_max/P_cmdall;
@ -255,13 +260,13 @@ void ChassisTask()
break;
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
chassis_cmd_recv.wz = 2.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
if(chassis_cmd_recv.wz > 3000)
chassis_cmd_recv.wz = 3000;
if(chassis_cmd_recv.wz < -3000)
chassis_cmd_recv.wz = -3000;
if(chassis_cmd_recv.wz > 4500)
chassis_cmd_recv.wz = 4500;
if(chassis_cmd_recv.wz < -4500)
chassis_cmd_recv.wz = -4500;
break;
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
chassis_cmd_recv.wz = 3000;
chassis_cmd_recv.wz = 4500;
break;
default:
break;
@ -272,9 +277,20 @@ void ChassisTask()
static float sin_theta, cos_theta;
cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx);
first_order_filter_cali(&vy_filter,chassis_cmd_recv.vy);
chassis_cmd_recv.vy = vy_filter.out;
chassis_cmd_recv.vx = vx_filter.out;
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
// 根据控制模式进行正运动学解算,计算底盘输出
MecanumCalculate();
//读取底盘功率，方便功率控制

68
application/cmd/robot_cmd.c
View File

@ -126,10 +126,9 @@ static void CalcOffsetAngle()
}
static void death_check()
{
if(referee_data->GameRobotState.remain_HP <= 0)
if(referee_data->GameRobotState.current_HP <= 0)
{
chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW;
gimbal_cmd_send.gimbal_mode = GIMBAL_FREE_MODE;
rc_data[TEMP].key_count[KEY_PRESS][Key_E] = 0;
}
}
@ -139,6 +138,8 @@ static void update_ui_data()
ui_data.gimbal_mode = gimbal_cmd_send.gimbal_mode;
ui_data.friction_mode = shoot_cmd_send.friction_mode;
ui_data.shoot_mode = shoot_cmd_send.shoot_mode;
ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
ui_data.Vision_fire = aim_select.suggest_fire;
}
static void pitch_limit()
{
@ -183,7 +184,7 @@ static void auto_aim_mode()
gimbal_cmd_send.yaw = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle + diff_yaw;
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI;
gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI + 0.2 * trajectory_cal.dis;
// float target_yaw = -atan2f(aim_select.armor_pose[target_index].x,
// aim_select.armor_pose[target_index].y);
@ -193,7 +194,7 @@ static void auto_aim_mode()
temp_yaw = target_yaw; //for test
float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw);
if (yaw_err <= 3) //3度
if (yaw_err <= 1) //3度
{
aim_select.suggest_fire = 1;
}
@ -264,11 +265,11 @@ static void RemoteControlSet()
shoot_cmd_send.friction_mode = FRICTION_OFF;
// 拨弹控制,遥控器固定为一种拨弹模式,可自行选择
if (rc_data[TEMP].rc.dial < -500)
shoot_cmd_send.load_mode = LOAD_1_BULLET;
shoot_cmd_send.load_mode = LOAD_STOP;
else
shoot_cmd_send.load_mode = LOAD_STOP;
// 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频,
shoot_cmd_send.shoot_rate = 10;
shoot_cmd_send.shoot_rate = 300;
}
/**
@ -277,26 +278,27 @@ static void RemoteControlSet()
*/
static void MouseKeySet()
{
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 20000 - rc_data[TEMP].key[KEY_PRESS].d * 20000; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 20000 - rc_data[TEMP].key[KEY_PRESS].s * 20000;
chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 30000 - rc_data[TEMP].key[KEY_PRESS].d * 30000; // 系数待测
chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 30000 - rc_data[TEMP].key[KEY_PRESS].s * 30000;
gimbal_cmd_send.yaw -= (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测
gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 *5 ;
aim_select.suggest_fire = 0;
if (rc_data[TEMP].mouse.press_l && (!rc_data[TEMP].mouse.press_r)) // 左键发射模式
{
if (shoot_cmd_send.friction_mode == FRICTION_ON) {
shoot_cmd_send.load_mode = LOAD_1_BULLET;
}
} else if ((!rc_data[TEMP].mouse.press_l) && (!rc_data[TEMP].mouse.press_r))
{
shoot_cmd_send.load_mode = LOAD_STOP;
}
if (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
{
if ((vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
&& vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
vision_recv_data->vz == 0))
@ -316,18 +318,16 @@ static void MouseKeySet()
}
}
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速//
// {
// case 0:
// shoot_cmd_send.bullet_speed = 15;
// break;
// case 1:
// shoot_cmd_send.bullet_speed = 18;
// break;
// default:
// shoot_cmd_send.bullet_speed = 30;
// break;
// }
switch (rc_data[TEMP].key[KEY_PRESS].v) // V键开启连发//
{
case 0:
shoot_cmd_send.shoot_rate= 300;
break;
case 1:
shoot_cmd_send.shoot_rate = 50;
break;
}
// switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式
@ -397,21 +397,6 @@ static void MouseKeySet()
gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
break;
}
// switch (rc_data[TEMP].key_count[KEY_PRESS][Key_C] % 4) // C键设置底盘速度
// {
// case 0:
// chassis_cmd_send.chassis_speed_buff = 40;
// break;
// case 1:
// chassis_cmd_send.chassis_speed_buff = 60;
// break;
// case 2:
// chassis_cmd_send.chassis_speed_buff = 80;
// break;
// default:
// chassis_cmd_send.chassis_speed_buff = 100;
// break;
// }
switch (rc_data[TEMP].key[KEY_PRESS].shift) // 待添加 按shift允许超功率 消耗缓冲能量
{
@ -484,9 +469,10 @@ void RobotCMDTask()
EmergencyHandler(); // 处理模块离线和遥控器急停等紧急情况
// 设置视觉发送数据,还需增加加速度和角速度数据
VisionSetFlag(chassis_fetch_data.enemy_color);
VisionSetFlag(!referee_data->referee_id.Robot_Color);
//根据裁判系统反馈确定底盘功率上限和缓冲功率
chassis_cmd_send.chassis_power_limit = referee_data->GameRobotState.chassis_power_limit;
chassis_cmd_send.chassis_power_buffer = referee_data->PowerHeatData.buffer_energy;
// 推送消息,双板通信,视觉通信等
// 其他应用所需的控制数据在remotecontrolsetmode和mousekeysetmode中完成设置
#ifdef ONE_BOARD

2
application/gimbal/gimbal.c
View File

@ -72,7 +72,7 @@ void GimbalInit()
.MaxOut = 500,//500
},
.speed_PID = {
.Kp = 2500,//11000, // 50
.Kp = 3500,//11000, // 50
.Ki = 1000, // 350
.Kd = 0,//15000, // 0
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,

7
application/robot_def.h
View File

@ -128,6 +128,7 @@ typedef enum
typedef struct
{ // 功率控制
float chassis_power_mx;
float last_power_mx;
} Chassis_Power_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@ -144,7 +145,8 @@ typedef struct
float wz; // 旋转速度
float offset_angle; // 底盘和归中位置的夹角
chassis_mode_e chassis_mode;
int chassis_speed_buff;
int chassis_power_buffer;
uint16_t chassis_power_limit;
// UI部分
// ...
@ -167,7 +169,7 @@ typedef struct
loader_mode_e load_mode;
lid_mode_e lid_mode;
friction_mode_e friction_mode;
Bullet_Speed_e bullet_speed; // 弹速枚举
float bullet_speed; // 发射周期
uint8_t rest_heat;
float shoot_rate; // 连续发射的射频,unit per s,发/秒
} Shoot_Ctrl_Cmd_s;
@ -189,7 +191,6 @@ typedef struct
// float real_wz;
uint8_t rest_heat; // 剩余枪口热量
Bullet_Speed_e bullet_speed; // 弹速限制
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
} Chassis_Upload_Data_s;

15
application/shoot/shoot.c
View File

@ -90,7 +90,7 @@ void ShootInit()
.Kd = 0,
.Improve = PID_Integral_Limit,
.IntegralLimit = 5000,
.MaxOut = 28000,
.MaxOut = 30000,
},
.current_PID = {
.Kp = 0, // 0.7
@ -165,9 +165,9 @@ void ShootTask()
stop_location = loader->measure.total_angle;
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成1发弹丸发射的时间
dead_time = shoot_cmd_recv.shoot_rate; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
// 自爆连发
case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
@ -183,6 +183,11 @@ void ShootTask()
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来
case LOAD_REVERSE:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle - ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成1发弹丸发射的时间
break;
break;
default:
while (1)
@ -193,9 +198,9 @@ void ShootTask()
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
{
DJIMotorSetRef(friction_l, 35001);
DJIMotorSetRef(friction_l, 48000);
// DJIMotorSetRef(friction_r, 39000);
DJIMotorSetRef(friction_z, 35000);//39000/6 = 6500
DJIMotorSetRef(friction_z, 48000);//39000/6 = 6500
}
else // 关闭摩擦轮
{

29
modules/algorithm/user_lib.c
View File

@ -212,3 +212,32 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
m->numRows = row;
m->pData = (float *)zmalloc(row * col * sizeof(float));
}
/**
* @brief 一阶低通滤波初始化
* @author RM
* @param[in] 一阶低通滤波结构体
* @param[in] 间隔的时间，单位 s
* @param[in] 滤波参数
* @retval 返回空
*/
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float32_t frame_period, const float32_t num[1])
{
first_order_filter_type->frame_period = frame_period;
first_order_filter_type->num[0] = num[0];
first_order_filter_type->input = 0.0f;
first_order_filter_type->out = 0.0f;
}
/**
* @brief 一阶低通滤波计算
* @author RM
* @param[in] 一阶低通滤波结构体
* @param[in] 间隔的时间，单位 s
* @retval 返回空
*/
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float32_t input)
{
first_order_filter_type->input = input;
first_order_filter_type->out =
first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
}

10
modules/algorithm/user_lib.h
View File

@ -32,6 +32,13 @@
#define mcos(x) (arm_cos_f32(x))
typedef arm_matrix_instance_f32 mat;
typedef __packed struct
{
float32_t input; //输入数据
float32_t out; //滤波输出的数据
float32_t num[1]; //滤波参数
float32_t frame_period; //滤波的时间间隔 单位 s
} first_order_filter_type_t;
// 若运算速度不够,可以使用q31代替f32,但是精度会降低
#define MatAdd arm_mat_add_f32
#define MatSubtract arm_mat_sub_f32
@ -120,7 +127,8 @@ void Cross3d(float *v1, float *v2, float *res);
float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len);
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float32_t input);
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float32_t frame_period, const float32_t num[1]);
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
#endif

97
modules/auto_aim/auto_aim.c
View File

@ -3,7 +3,7 @@
//
#include "auto_aim.h"
#include "arm_math.h"
uint8_t mode_flag=0;
/**
* @brief 选择目标装甲板（瞄装甲板）
* @author SJQ
@ -58,25 +58,24 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
}
// 选择两块较近的装甲板
float distance_there[3];
float distance[3];
for (i = 0; i < 3; i++) {
distance_there[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
}
int label_first = 0;
int label_second = 0;
int label_second = 1;
for (i = 1; i < 3; i++) {
if (distance_there[i] < distance_there[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance_there[i] < distance_there[label_second] && distance_there[i] < distance_there[label_first]) {
label_second = i;
} else if (distance_there[i] < distance_there[label_second]) {
label_second = i;
}
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
@ -118,25 +117,24 @@ int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_c
// }
// 选择两块较近的装甲板
float distance_four[4];
for (i = 0; i < 4; i++) {
distance_four[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
float distance[3];
for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
}
int label_first = 0;
int label_second = 0;
int label_second = 1;
for (i = 1; i < 4; i++) {
if (distance_four[i] < distance_four[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance_four[i] < distance_four[label_second] && distance_four[i] < distance_four[label_first]) {
label_second = i;
}else if (distance_four[i] < distance_four[label_second]) {
label_second = i;
}
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 2));
@ -256,19 +254,18 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
}
int label_first = 0;
int label_second = 0;
int label_second = 1;
for (i = 1; i < 3; i++) {
if (distance[i] < distance[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance[i] < distance[label_second]) {
label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
label_second = i;
}
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
@ -309,25 +306,25 @@ int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_
// }
// 选择两块较近de装甲板
float distance[4];
for (i = 0; i < 4; i++) {
// 选择两块较近的装甲板
float distance[3];
for (i = 0; i < 3; i++) {
distance[i] = powf(aim_sel->armor_pose[i].x, 2) + powf(aim_sel->armor_pose[i].y, 2);
}
int label_first = 0;
int label_second = 0;
int label_second = 1;
for (i = 1; i < 4; i++) {
if (distance[i] < distance[label_first]) {
label_second = label_first;
label_first = i;
} else if (distance[i] < distance[label_second]) {
label_second = i;
} else if (distance[i] < distance[label_second] && distance[i] < distance[label_first]) {
label_second = i;
}
if(distance[label_first] > distance[label_second])
{
label_first = 1;
label_second = 0;
}
if(distance[2]<distance[label_second])
label_second = 2;
//再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板
float cos_theta_first = (aim_sel->armor_pose[label_first].x * aim_sel->target_state.x +
aim_sel->armor_pose[label_first].y * aim_sel->target_state.y) /
@ -436,14 +433,12 @@ int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Reci
aim_sel->target_state.dz = receive_packet->dz;
int idx = -1;
if(aim_sel->target_state.v_yaw >= 0.7* PI)
if(fabsf(aim_sel->target_state.v_yaw) >= 0.7* PI)
{
mode_flag=0;
idx = aim_center_select(aim_sel, trajectory_cal);
}
else
{
mode_flag=1;
idx = aim_armor_select(aim_sel, trajectory_cal);
}

11
modules/master_machine/master_process.h
View File

@ -61,21 +61,12 @@ typedef enum
VISION_MODE_BIG_BUFF = 2
} Work_Mode_e;
typedef enum
{
BULLET_SPEED_NONE = 0,
BIG_AMU_10 = 10,
SMALL_AMU_15 = 15,
BIG_AMU_16 = 16,
SMALL_AMU_18 = 18,
SMALL_AMU_30 = 30,
} Bullet_Speed_e;
typedef struct
{
Enemy_Color_e enemy_color;
Work_Mode_e work_mode;
Bullet_Speed_e bullet_speed;
float yaw;
float pitch;

7
modules/motor/DJImotor/dji_motor.c
View File

@ -231,13 +231,14 @@ void DJIMotorSetRef(DJIMotorInstance *motor, float ref)
motor->motor_controller.pid_ref = ref;
}
//
static const float motor_power_K[3] = {1.99688994e-6f,1.453e-7f,1.23e-7f};
static const float motor_power_K[3] = {1.6301e-6f,5.7501e-7f,2.5863e-7f};
//依据3508电机功率模型，预测电机输出功率
static float EstimatePower(DJIMotorInstance* chassis_motor)
static float EstimatePower(DJIMotorInstance* chassis_motor,float output)
{
float I_cmd = chassis_motor->motor_controller.current_PID.Output;
float w = chassis_motor->measure.speed_aps /6 ;//aps to rpm
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd+1.021f;
float power = motor_power_K[0] * I_cmd * w + motor_power_K[1]*w*w + motor_power_K[2]*I_cmd*I_cmd;
return power;
}
// 为所有电机实例计算三环PID,发送控制报文

29
modules/referee/referee_protocol.h
View File

@ -157,37 +157,32 @@ typedef struct
uint8_t supply_projectile_num;
} ext_supply_projectile_action_t;
/* ID: 0X0201 Byte: 27 机器人状态数据 */
/* ID: 0X0201 Byte: 27 V1.6.1 机器人状态数据 */
typedef struct
{
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_HP;
uint16_t max_HP;
uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_id1_17mm_cooling_limit;
uint16_t shooter_id1_17mm_speed_limit;
uint16_t shooter_id2_17mm_cooling_rate;
uint16_t shooter_id2_17mm_cooling_limit;
uint16_t shooter_id2_17mm_speed_limit;
uint16_t shooter_id1_42mm_cooling_rate;
uint16_t shooter_id1_42mm_cooling_limit;
uint16_t shooter_id1_42mm_speed_limit;
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 mains_power_gimbal_output : 1;
uint8_t mains_power_chassis_output : 1;
uint8_t mains_power_shooter_output : 1;
} ext_game_robot_state_t;
/* ID: 0X0202 Byte: 14 实时功率热量数据 */
/* ID: 0X0202 Byte: 14 V1.6.1实时功率热量数据 */
typedef struct
{
uint16_t chassis_volt;
uint16_t chassis_voltage;
uint16_t chassis_current;
float chassis_power; // 瞬时功率
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
uint16_t shooter_heat0; // 17mm
uint16_t shooter_heat1;
uint16_t buffer_energy; // 60焦耳缓冲能量
uint16_t shooter_17mm_1_barrel_heat;
uint16_t shooter_17mm_2_barrel_heat;
uint16_t shooter_42mm_barrel_heat;
} ext_power_heat_data_t;
/* ID: 0x0203 Byte: 16 机器人位置数据 */

50
modules/referee/referee_task.c
View File

@ -55,15 +55,16 @@ void UITask()
}
static Graph_Data_t UI_shoot_line[10]; // 射击准线
static Graph_Data_t UI_shoot_circle[1]; //自瞄标识
static Graph_Data_t UI_Energy[3]; // 电容能量条
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565,940};
void MyUIInit()
{
if (!referee_recv_info->init_flag)
vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
// if (!referee_recv_info->init_flag)
// vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
while (referee_recv_info->GameRobotState.robot_id == 0)
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
@ -77,7 +78,9 @@ void MyUIInit()
UILineDraw(&UI_shoot_line[2], "sl2", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[2], 1110, shoot_line_location[2]);
UILineDraw(&UI_shoot_line[3], "sl3", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[3], 1110, shoot_line_location[3]);
UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_ADD, 7, UI_Color_Yellow, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4]);
UILineDraw(&UI_shoot_line[5], "sl5", UI_Graph_ADD, 7, UI_Color_Yellow, 2, shoot_line_location[5], 200, shoot_line_location[5], 800);
UIGraphRefresh(&referee_recv_info->referee_id, 7, UI_shoot_line[0], UI_shoot_line[1], UI_shoot_line[2], UI_shoot_line[3], UI_shoot_line[4], UI_shoot_line[5],UI_shoot_line[6]);
// 绘制车辆状态标志指示
@ -106,16 +109,17 @@ void MyUIInit()
// UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
// 底盘功率显示，静态
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power_MAX:");
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
// 能量条框
UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
// 底盘功率显示,动态
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 750, 230, 24000);
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 2, 850, 230, referee_recv_info->GameRobotState.chassis_power_limit*1000);
// 能量条初始状态
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
// UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
}
@ -249,11 +253,30 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
// power
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
{
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 850, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
// UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
}
//line
if (_Interactive_data->Referee_Interactive_Flag.Vision_flag == 1)
{
switch (_Interactive_data->Vision_fire) {
case 0 :
UICircleDraw(&UI_shoot_circle[0],"sc0",UI_Graph_Del,6,UI_Color_Green,2,960,540,20);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_circle[0]);
break;
case 1 :
UICircleDraw(&UI_shoot_circle[0],"sc0",UI_Graph_ADD,6,UI_Color_Green,2,960,540,20);
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_circle[0]);
break;
}
// UILineDraw(&UI_shoot_line[4], "sl4", UI_Graph_Change, 7, _Interactive_data->Vision_fire == 0 ? UI_Color_Yellow : UI_Color_Purplish_red, 2, 810, shoot_line_location[4], 1110, shoot_line_location[4]);
// UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_shoot_line[4]);
// _Interactive_data->Referee_Interactive_Flag.Vision_flag = 0;
}
}
/**
@ -294,9 +317,14 @@ static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data)
_Interactive_data->lid_last_mode = _Interactive_data->lid_mode;
}
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.chassis_power_mx)
if (_Interactive_data->Chassis_Power_Data.chassis_power_mx != _Interactive_data->Chassis_last_Power_Data.last_power_mx)
{
_Interactive_data->Referee_Interactive_Flag.Power_flag = 1;
_Interactive_data->Chassis_last_Power_Data.chassis_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
_Interactive_data->Chassis_last_Power_Data.last_power_mx = _Interactive_data->Chassis_Power_Data.chassis_power_mx;
}
if (_Interactive_data->Vision_fire != _Interactive_data->last_Vision_fire)
{
_Interactive_data->Referee_Interactive_Flag.Vision_flag = 1;
_Interactive_data->last_Vision_fire = _Interactive_data->Vision_fire;
}
}

3
modules/referee/rm_referee.h
View File

@ -54,6 +54,7 @@ typedef struct
uint32_t lid_flag : 1;
uint32_t friction_flag : 1;
uint32_t Power_flag : 1;
uint32_t Vision_flag :1;
} Referee_Interactive_Flag_t;
// 此结构体包含UI绘制与机器人车间通信的需要的其他非裁判系统数据
@ -67,6 +68,7 @@ typedef struct
friction_mode_e friction_mode; // 摩擦轮关闭
lid_mode_e lid_mode; // 弹舱盖打开
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
uint8_t Vision_fire;
// 上一次的模式，用于flag判断
@ -76,6 +78,7 @@ typedef struct
friction_mode_e friction_last_mode;
lid_mode_e lid_last_mode;
Chassis_Power_Data_s Chassis_last_Power_Data;
uint8_t last_Vision_fire;
} Referee_Interactive_info_t;