From b0efcd64ffcf49f88e20dd074459ac49dbf22b84 Mon Sep 17 00:00:00 2001 From: zcj <2487150395@qq.com> Date: Wed, 3 Apr 2024 01:12:50 +0800 Subject: [PATCH] =?UTF-8?q?=E8=81=94=E7=9B=9F=E8=B5=9B=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 32 ++++++-- application/cmd/robot_cmd.c | 68 +++++++---------- application/gimbal/gimbal.c | 2 +- application/robot_def.h | 7 +- application/shoot/shoot.c | 15 ++-- modules/algorithm/user_lib.c | 29 ++++++++ modules/algorithm/user_lib.h | 10 ++- modules/auto_aim/auto_aim.c | 97 ++++++++++++------------- modules/master_machine/master_process.h | 11 +-- modules/motor/DJImotor/dji_motor.c | 7 +- modules/referee/referee_protocol.h | 29 +++----- modules/referee/referee_task.c | 50 ++++++++++--- modules/referee/rm_referee.h | 3 + 13 files changed, 209 insertions(+), 151 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 1123af7..a0ed56a 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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(); //读取底盘功率，方便功率控制 diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index edd1c18..d1d729d 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -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 diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index de760c6..1442a93 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -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, diff --git a/application/robot_def.h b/application/robot_def.h index 4e1b1d5..1eabef8 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -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; diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 76180bb..38b6493 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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 // 关闭摩擦轮 { diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 5029109..616787c 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -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; +} \ No newline at end of file diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 9a15097..5e85f95 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -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 diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 3c53532..6aae159 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -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]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]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]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]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); } diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index c50c2b4..5f9a7ba 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -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; diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index a6e88e2..b41ddb9 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -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,发送控制报文 diff --git a/modules/referee/referee_protocol.h b/modules/referee/referee_protocol.h index 22416b3..c37b8eb 100644 --- a/modules/referee/referee_protocol.h +++ b/modules/referee/referee_protocol.h @@ -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 机器人位置数据 */ diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 15573de..9910da1 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -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; } } diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index 30c0d1f..b909609 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -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;