From 51982171c2213c83f58f4284098a73f9b2659eac Mon Sep 17 00:00:00 2001 From: sph <1527550984@qq.com> Date: Sat, 9 Mar 2024 21:54:02 +0800 Subject: [PATCH] =?UTF-8?q?=E7=81=AB=E6=8E=A7=E4=BF=AE=E6=94=B9=E3=80=81?= =?UTF-8?q?=E9=87=8D=E6=9E=84=EF=BC=8C=E5=BE=85=E9=87=8D=E6=96=B0=E6=B5=8B?= =?UTF-8?q?=E8=AF=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/gimbal/gimbal.c | 28 +++++++- application/robot_def.h | 5 ++ application/shoot/shoot.c | 4 +- modules/auto_aim/auto_aim.c | 135 +++++++++++++++++++++++++++++++----- 4 files changed, 150 insertions(+), 22 deletions(-) diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 5dc40f2..34e65ee 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -4,8 +4,8 @@ #include "ins_task.h" #include "message_center.h" #include "general_def.h" - #include "bmi088.h" +#include "vofa.h" static attitude_t *gimba_IMU_data; // 云台IMU数据 static DJIMotorInstance *yaw_motor, *pitch_motor; @@ -14,9 +14,32 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者 static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 -#include "vofa.h" +static void LimitPitchAngleAuto() { + /** 注意电机角度与陀螺仪角度方向是否相同 + * 目前 add > 0, 向下转动 + * 电机角度值减小 + * 陀螺仪角度值增大 + **/ + float add; + float angle_set; + + add = gimbal_cmd_recv.pitch - gimba_IMU_data->Pitch; + + if(pitch_motor->measure.angle_single_round - add > PITCH_MAX_RELATIVE_ANGLE){ + if(add < 0.0f ){ + add = PITCH_MAX_ANGLE - pitch_motor->measure.angle_single_round; + } + }else if(pitch_motor->measure.angle_single_round - add < PITCH_MIN_RELATIVE_ANGLE){ + if(add > 0.0f){ + add = PITCH_MIN_RELATIVE_ANGLE - pitch_motor->measure.angle_single_round; + } + } + angle_set = gimba_IMU_data->Pitch; + DJIMotorSetRef(pitch_motor, angle_set+add); +} + void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 // YAW @@ -127,6 +150,7 @@ void GimbalTask() { DJIMotorChangeFeed(pitch_motor, SPEED_LOOP, OTHER_FEED); DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈 DJIMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + //LimitPitchAngleAuto(); break; // 云台自由模式,使用编码器反馈,底盘和云台分离,仅云台旋转,一般用于调整云台姿态(英雄吊射等)/能量机关 case GIMBAL_FREE_MODE: // 后续删除,或加入云台追地盘的跟随模式(响应速度更快) diff --git a/application/robot_def.h b/application/robot_def.h index 407929f..490fff0 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -31,6 +31,11 @@ #define PITCH_HORIZON_ECD 1670 // 云台处于水平位置时编码器值,若对云台有机械改动需要修改 #define PITCH_MAX_ANGLE 25 // 云台竖直方向最大角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) #define PITCH_MIN_ANGLE -18 // 云台竖直方向最小角度 (注意反馈如果是陀螺仪，则填写陀螺仪的角度) + +#define PITCH_MAX_RELATIVE_ANGLE 123 // 云台相对底盘最大角度 +#define PITCH_MIN_RELATIVE_ANGLE 80 // 云台相对底盘最小角度 + + // 发射参数 #define ONE_BULLET_DELTA_ANGLE 45 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 #define REDUCTION_RATIO_LOADER 49.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index b75959d..bbdafb4 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -217,8 +217,8 @@ void ShootTask() // DJIMotorSetRef(friction_r, 0); // break; //default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 30000); - DJIMotorSetRef(friction_r, 30000); + DJIMotorSetRef(friction_l, 34000); + DJIMotorSetRef(friction_r, 34000); // break; // } } diff --git a/modules/auto_aim/auto_aim.c b/modules/auto_aim/auto_aim.c index 695f3a7..9d0f257 100644 --- a/modules/auto_aim/auto_aim.c +++ b/modules/auto_aim/auto_aim.c @@ -30,14 +30,22 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ 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; +// 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++) { @@ -55,15 +63,70 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// //计算枪管到目标装甲板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++) { - 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; + 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; @@ -77,15 +140,51 @@ void aim_armor_select(Aim_Select_Type_t *aim_sel, Trajectory_Type_t *trajectory_ use_1 = !use_1; } - //计算枪管到目标装甲板yaw最小的那个装甲板 - float yaw_diff_min = fabsf(trajectory_cal->cmd_yaw - aim_sel->armor_pose[0].yaw); +// //计算枪管到目标装甲板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++) { - 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; + 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;