From 2b402788d1052161fbb449978080f6a2aa2ccf7a Mon Sep 17 00:00:00 2001 From: zcj <2487150395@qq.com> Date: Sat, 23 Mar 2024 07:44:19 +0800 Subject: [PATCH] =?UTF-8?q?=E8=87=AA=E7=9E=84=E4=B8=8A=E6=93=8D=E4=BD=9C?= =?UTF-8?q?=E7=AB=AF=E5=B9=B6=E4=BC=98=E5=8C=96=EF=BC=8Cpitch=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3=E5=A4=B1=E8=B4=A5=EF=BC=8Cui=E5=88=B7=E6=96=B0?= =?UTF-8?q?=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/chassis.c | 18 +-- application/cmd/robot_cmd.c | 171 ++++++++++++++++----------- application/gimbal/gimbal.c | 15 ++- application/shoot/shoot.c | 20 ++-- modules/auto_aim/auto_aim.c | 207 ++++++++++++++++++++++++++++++++- modules/referee/referee_task.c | 8 +- modules/referee/rm_referee.h | 1 + 7 files changed, 339 insertions(+), 101 deletions(-) diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index d7799d3..d761d78 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -160,10 +160,10 @@ void ChassisInit() */ static void MecanumCalculate() { - vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER-1); - vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER-1); - vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER-1); - vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER-1); + vt_lf = chassis_vx + chassis_vy - chassis_cmd_recv.wz * (LF_CENTER); + vt_rf = -chassis_vx + chassis_vy + chassis_cmd_recv.wz * (RF_CENTER); + vt_lb = chassis_vx - chassis_vy - chassis_cmd_recv.wz * (-LB_CENTER); + vt_rb = -chassis_vx - chassis_vy + chassis_cmd_recv.wz * (-RB_CENTER); } @@ -255,13 +255,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 > 2000) - chassis_cmd_recv.wz = 2000; - if(chassis_cmd_recv.wz < -2000) - chassis_cmd_recv.wz = -2000; + if(chassis_cmd_recv.wz > 3000) + chassis_cmd_recv.wz = 3000; + if(chassis_cmd_recv.wz < -3000) + chassis_cmd_recv.wz = -3000; break; case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略 - chassis_cmd_recv.wz = 2300; + chassis_cmd_recv.wz = 3000; break; default: break; diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index ed2c1c2..d10dc5b 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -133,9 +133,61 @@ static void pitch_limit() // gimbal_cmd_send.pitch -= 0.1f; // if(gimbal_fetch_data.pitch_motor_total_angle < horizontal_angle - 4000) // gimbal_cmd_send.pitch += 0.1f; - if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18; - if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38; + if(gimbal_cmd_send.pitch>18) gimbal_cmd_send.pitch = 18; + if(gimbal_cmd_send.pitch<-38) gimbal_cmd_send.pitch = -38; } +static void hand_aim_mode() +{ + gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测 + gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9; + pitch_limit(); +} +static void auto_aim_mode() +{ + trajectory_cal.v0 = 30; //弹速30 + 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) { + aim_select.suggest_fire = 0; + //未发现目标 + no_find_cnt++; + + if (no_find_cnt >= 2000) { + //gimbal_scan_flag = 1; + //auto_aim_flag = 0; + } + //else + //auto_aim_flag = 1; + } else { + //弹道解算 + no_find_cnt = 0; + auto_aim_flag = 1; + + target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); + VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]); + + gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI; + + gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI; + + float target_yaw = atan2f(aim_select.armor_pose[target_index].x, + aim_select.armor_pose[target_index].y); + float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw); + if (yaw_err <= 3) //3度 + { + aim_select.suggest_fire = 1; + } + else + aim_select.suggest_fire = 0; + +// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); +// if (yaw_err <= 3) //3度 +// aim_select.suggest_fire = 1; +// else +// aim_select.suggest_fire = 0; + } +} + + /** * @brief 控制输入为遥控器(调试时)的模式和控制量设置 * @@ -169,45 +221,7 @@ static void RemoteControlSet() } // 左侧开关状态为[下],视觉模式 if (switch_is_down(rc_data[TEMP].rc.switch_left)) { - trajectory_cal.v0 = 12.5; //弹速30 - 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) { - aim_select.suggest_fire = 0; - //未发现目标 - no_find_cnt++; - - if (no_find_cnt >= 2000) { - //gimbal_scan_flag = 1; - //auto_aim_flag = 0; - } - //else - //auto_aim_flag = 1; - } else { - //弹道解算 - no_find_cnt = 0; - auto_aim_flag = 1; - - target_index = auto_aim(&aim_select, &trajectory_cal, vision_recv_data); - VisionSetAim(aim_select.aim_point[0],aim_select.aim_point[1],aim_select.aim_point[2]); - - gimbal_cmd_send.yaw = -trajectory_cal.cmd_yaw * 180 / PI; - - gimbal_cmd_send.pitch = -trajectory_cal.cmd_pitch * 180 / PI; - - float target_yaw = atan2f(aim_select.armor_pose[target_index].x, - aim_select.armor_pose[target_index].y); - float yaw_err = fabsf(target_yaw - gimbal_fetch_data.gimbal_imu_data.Yaw); - if (yaw_err <= 3) //3度 - aim_select.suggest_fire = 1; - else - aim_select.suggest_fire = 0; - -// float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw); -// if (yaw_err <= 3) //3度 -// aim_select.suggest_fire = 1; -// else -// aim_select.suggest_fire = 0; - } + auto_aim_mode(); } // 云台软件限位 @@ -242,12 +256,10 @@ static void RemoteControlSet() */ static void MouseKeySet() { - chassis_cmd_send.vx = rc_data[TEMP].key[KEY_PRESS].a * 6000 - rc_data[TEMP].key[KEY_PRESS].d * 6000; // 系数待测 - chassis_cmd_send.vy = rc_data[TEMP].key[KEY_PRESS].w * 6000 - rc_data[TEMP].key[KEY_PRESS].s * 6000; + 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; + - gimbal_cmd_send.yaw += (float)rc_data[TEMP].mouse.x / 660 * 9; // 系数待测 - gimbal_cmd_send.pitch += (float)rc_data[TEMP].mouse.y / 660 * 9; - pitch_limit(); // switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Z] % 3) // Z键设置弹速// // { // case 0: @@ -260,11 +272,37 @@ static void MouseKeySet() // shoot_cmd_send.bullet_speed = 30; // break; // } + + switch (rc_data[TEMP].mouse.press_r) // 右键自瞄模式 + { + case 1: + 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)) + hand_aim_mode(); + else + auto_aim_mode(); + break; + default: + hand_aim_mode(); + break; + } + switch (rc_data[TEMP].mouse.press_l) // 左键发射模式 { case 1: if(shoot_cmd_send.friction_mode == FRICTION_ON) + { + if(rc_data[TEMP].mouse.press_r && rc_data[TEMP].mouse.press_l) + { + if(aim_select.suggest_fire == 1) + shoot_cmd_send.load_mode = LOAD_1_BULLET; + else + shoot_cmd_send.load_mode = LOAD_STOP; + break; + } shoot_cmd_send.load_mode = LOAD_1_BULLET; + } else shoot_cmd_send.load_mode = LOAD_STOP; break; @@ -272,13 +310,14 @@ static void MouseKeySet() shoot_cmd_send.load_mode = LOAD_STOP; break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键开关弹舱 + + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_R] % 2) // R键手动刷新UI { - case 0: - shoot_cmd_send.lid_mode = LID_OPEN; + case 1: + MyUIInit(); + rc_data[TEMP].key_count[KEY_PRESS][Key_R]++; break; default: - shoot_cmd_send.lid_mode = LID_CLOSE; break; } switch (rc_data[TEMP].key_count[KEY_PRESS][Key_F] % 2) // F键开关摩擦轮 @@ -290,7 +329,7 @@ static void MouseKeySet() shoot_cmd_send.friction_mode = FRICTION_ON; break; } - switch (rc_data[TEMP].key_count[KEY_PRESS][Key_Q] % 2) // Q键开关小陀螺 + switch (rc_data[TEMP].key_count[KEY_PRESS][Key_E] % 2) // E键开关小陀螺 { case 0: chassis_cmd_send.chassis_mode = CHASSIS_FOLLOW_GIMBAL_YAW; @@ -301,21 +340,21 @@ 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_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允许超功率 消耗缓冲能量 { case 1: diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index a9c1766..1062122 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -27,10 +27,9 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 40, // 8 + .Kp = 200, // 8 .Ki = 0, - .Kd = 5, - .DeadBand = 0.1, + .Kd = 20, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100, @@ -38,8 +37,8 @@ void GimbalInit() }, .speed_PID = { .Kp = 70, // 70 - .Ki = 200, // 200 - .Kd = 10,//10 + .Ki = 0, // 200 + .Kd = 0,//10 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 3000, .MaxOut = 20000, @@ -65,8 +64,8 @@ void GimbalInit() }, .controller_param_init_config = { .angle_PID = { - .Kp = 15, // 10 - .Ki = 0.1f, + .Kp = 20, // 65 + .Ki = 0, .Kd = 1, .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 100,//100 @@ -75,7 +74,7 @@ void GimbalInit() .speed_PID = { .Kp = 30, // 50 .Ki = 0, // 350 - .Kd = 1, // 0 + .Kd = 2, // 0 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, .IntegralLimit = 150,//2500 .MaxOut = 2000,//20000 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 8ca2d3d..6f75ceb 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -55,15 +55,15 @@ void ShootInit() .outer_loop_type = SPEED_LOOP, .close_loop_type = SPEED_LOOP , - .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, + .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, .motor_type = M3508}; friction_config.can_init_config.tx_id = 3, friction_l = DJIMotorInit(&friction_config); - friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行 - friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; - friction_r = DJIMotorInit(&friction_config); +// friction_config.can_init_config.tx_id = 4; // 右摩擦轮,改txid和方向就行 +// friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; +// friction_r = DJIMotorInit(&friction_config); friction_config.can_init_config.tx_id = 6; // 上摩擦轮,改txid和方向就行 friction_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE; @@ -135,14 +135,14 @@ void ShootTask() if (shoot_cmd_recv.shoot_mode == SHOOT_OFF) { DJIMotorStop(friction_l); - DJIMotorStop(friction_r); +// DJIMotorStop(friction_r); DJIMotorStop(friction_z); DJIMotorStop(loader); } else // 恢复运行 { DJIMotorEnable(friction_l); - DJIMotorEnable(friction_r); +// DJIMotorEnable(friction_r); DJIMotorEnable(friction_z); DJIMotorEnable(loader); } @@ -193,14 +193,14 @@ void ShootTask() // 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启) if (shoot_cmd_recv.friction_mode == FRICTION_ON) { - DJIMotorSetRef(friction_l, 39000); - DJIMotorSetRef(friction_r, 39000); - DJIMotorSetRef(friction_z, 39000);//39000/6 = 6500 + DJIMotorSetRef(friction_l, 36000); +// DJIMotorSetRef(friction_r, 39000); + DJIMotorSetRef(friction_z, 36000);//39000/6 = 6500 } else // 关闭摩擦轮 { DJIMotorSetRef(friction_l, 0); - DJIMotorSetRef(friction_r, 0); +// DJIMotorSetRef(friction_r, 0); DJIMotorSetRef(friction_z, 0); } diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index a9e2bec..6e78c58 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -3,14 +3,203 @@ // #include "auto_aim.h" #include "arm_math.h" +uint8_t mode_flag=0; +/** + * @brief 选择目标装甲板（瞄装甲板） + * @author SJQ + * @param[in] trajectory_cal:弹道解算结构体 + * @retval 返回空 + */ +int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { + aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time; + aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time; + //计算四块装甲板的位置 + int use_1 = 1; + int i = 0; + int idx = 0; // 选择的装甲板 + // 为平衡步兵 + if (aim_sel->target_state.armor_num == 2) { + for (i = 0; i < 2; i++) { + float tmp_yaw = aim_sel->target_state.yaw + i * PI; + float r = aim_sel->target_state.r1; + + aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw); + aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw); + aim_sel->armor_pose[i].z = aim_sel->target_state.z; + aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * PI; + } + +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// +// //因为是平衡步兵 只需判断两块装甲板即可 +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[1].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = 1; +// } + // 平衡步兵选择两块装甲板中较近的装甲板 + float distance_min = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); + float distance_temp = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); + if (distance_temp < distance_min) { + distance_min = distance_temp; + idx = 1; + } + + } else if (aim_sel->target_state.armor_num == 3)//前哨站 + { + for (i = 0; i < 3; i++) { + float tmp_yaw; + if (aim_sel->target_state.v_yaw <= 0) + tmp_yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); + else + tmp_yaw = aim_sel->target_state.yaw - i * (2.0 * PI / 3.0); + float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; + aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * cos(tmp_yaw); + aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * sin(tmp_yaw); + aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z + + aim_sel->target_state.dz; + aim_sel->armor_pose[i].yaw = aim_sel->target_state.yaw + i * (2.0 * PI / 3.0); + use_1 = !use_1; + } + +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 3; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + // 选择两块较近的装甲板，再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 +// float distance_temp0 = powf(aim_sel->armor_pose[0].x, 2) + powf(aim_sel->armor_pose[0].y, 2); +// float distance_temp1 = powf(aim_sel->armor_pose[1].x, 2) + powf(aim_sel->armor_pose[1].y, 2); +// float distance_temp2 = powf(aim_sel->armor_pose[2].x, 2) + powf(aim_sel->armor_pose[2].y, 2); + +// int label_first = 0; +// int label_second = 1; +// if (distance_temp0 > distance_temp1) { +// label_first = 1; +// if (distance_temp0 > distance_temp2) { +// label_second = 2; +// } else label_second = 0; +// } else { +// label_first = 0; +// if (distance_temp1 > distance_temp2) { +// label_second = 2; +// } else label_second = 1; +// } + + // 选择两块较近的装甲板 + 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; + + 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; + } + } + + //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 + float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 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) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; + + } else { + for (i = 0; i < 4; i++) { + float tmp_yaw = aim_sel->target_state.yaw + i * PI / 2.0; + float r = use_1 ? aim_sel->target_state.r1 : aim_sel->target_state.r2; + + aim_sel->armor_pose[i].x = aim_sel->target_state.x - r * arm_cos_f32(tmp_yaw); + aim_sel->armor_pose[i].y = aim_sel->target_state.y - r * arm_sin_f32(tmp_yaw); + aim_sel->armor_pose[i].z = use_1 ? aim_sel->target_state.z : aim_sel->target_state.z + + aim_sel->target_state.dz; + aim_sel->armor_pose[i].yaw = tmp_yaw; + use_1 = !use_1; + } + +// //计算枪管到目标装甲板yaw最小的那个装甲板 +// float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// for (i = 1; i < 4; i++) { +// float temp_yaw_diff = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[i].yaw); +// if (temp_yaw_diff < yaw_diff_min) { +// yaw_diff_min = temp_yaw_diff; +// idx = i; +// } +// } + + // 选择两块较近的装甲板 + float distance[4]; + for (i = 0; i < 4; 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; + + 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; + } + } + + //再选择两块装甲板与自身位置连线，同旋转中心自身位置连线，夹角较小的为目标装甲板 + float center_length = sqrtf(powf(aim_sel->target_state.x, 2) + powf(aim_sel->target_state.y, 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) / + (sqrtf(powf(aim_sel->armor_pose[label_first].x, 2) + + powf(aim_sel->armor_pose[label_first].y, 2)) * center_length); + float cos_theta_second = (aim_sel->armor_pose[label_second].x * aim_sel->target_state.x + + aim_sel->armor_pose[label_second].y * aim_sel->target_state.y) / + (sqrtf(powf(aim_sel->armor_pose[label_second].x, 2) + + powf(aim_sel->armor_pose[label_second].y, 2)) * center_length); + + if (cos_theta_first > cos_theta_second) + idx = label_first; + else + idx = label_second; + } + aim_sel->aim_point[0] = aim_sel->armor_pose[idx].x + aim_sel->target_state.vx * aim_sel->delay_time; + aim_sel->aim_point[1] = aim_sel->armor_pose[idx].y + aim_sel->target_state.vy * aim_sel->delay_time; + aim_sel->aim_point[2] = aim_sel->armor_pose[idx].z + aim_sel->target_state.vz * aim_sel->delay_time; + return idx; +} /** * @brief 选择目标装甲板 * @author SJQ * @param[in] trajectory_cal:弹道解算结构体 * @retval 返回空 */ -int aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { +int aim_center_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal) { aim_sel->delay_time = trajectory_cal->fly_time + trajectory_cal->extra_delay_time; aim_sel->target_state.yaw += aim_sel->target_state.v_yaw * aim_sel->delay_time; @@ -229,7 +418,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { arm_power_f32(trajectory_cal->position_xy, 2, &trajectory_cal->dis2); arm_sqrt_f32(trajectory_cal->dis2, &trajectory_cal->dis); - trajectory_cal->dis = trajectory_cal->dis - 0.20; +// trajectory_cal->dis = trajectory_cal->dis - 0.20; trajectory_cal->theta_0 = atan2f(trajectory_cal->z, trajectory_cal->dis); trajectory_cal->alpha = atan2f(trajectory_cal->position_xy[1], trajectory_cal->position_xy[0]); @@ -264,7 +453,7 @@ void get_cmd_angle(Trajectory_Type_t *trajectory_cal) { } int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, RecievePacket_t *receive_packet) { - trajectory_cal->extra_delay_time = 0.025f;//0.025 + trajectory_cal->extra_delay_time = 0.035f;//0.025 aim_sel->target_state.armor_type = receive_packet->id; aim_sel->target_state.armor_num = receive_packet->armors_num; @@ -280,7 +469,17 @@ int auto_aim(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_cal, Reci aim_sel->target_state.r2 = receive_packet->r2; aim_sel->target_state.dz = receive_packet->dz; - int idx = aim_armor_select(aim_sel, trajectory_cal); + int idx = 0; + if(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); + } trajectory_cal->position_xy[0] = aim_sel->aim_point[0]; trajectory_cal->position_xy[1] = aim_sel->aim_point[1]; diff --git a/modules/referee/referee_task.c b/modules/referee/referee_task.c index 8be64a5..15573de 100644 --- a/modules/referee/referee_task.c +++ b/modules/referee/referee_task.c @@ -62,8 +62,8 @@ static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565}; 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); // 若还未收到裁判系统数据,等待一段时间后再检查 @@ -94,9 +94,9 @@ void MyUIInit() // 绘制车辆状态标志，动态 // 由于初始化时xxx_last_mode默认为0，所以此处对应UI也应该设为0时对应的UI，防止模式不变的情况下无法置位flag，导致UI无法刷新 - UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "zeroforce"); + UICharDraw(&UI_State_dyn[0], "sd0", UI_Graph_ADD, 8, UI_Color_Main, 15, 2, 270, 750, "follow"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[0]); - UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "zeroforce"); + UICharDraw(&UI_State_dyn[1], "sd1", UI_Graph_ADD, 8, UI_Color_Yellow, 15, 2, 270, 700, "free"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[1]); UICharDraw(&UI_State_dyn[2], "sd2", UI_Graph_ADD, 8, UI_Color_Orange, 15, 2, 270, 650, "off"); UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[2]); diff --git a/modules/referee/rm_referee.h b/modules/referee/rm_referee.h index e935e94..30c0d1f 100644 --- a/modules/referee/rm_referee.h +++ b/modules/referee/rm_referee.h @@ -68,6 +68,7 @@ typedef struct lid_mode_e lid_mode; // 弹舱盖打开 Chassis_Power_Data_s Chassis_Power_Data; // 功率控制 + // 上一次的模式，用于flag判断 chassis_mode_e chassis_last_mode; gimbal_mode_e gimbal_last_mode;