From bbff75512424f8a9537c17f37bdafd8bc7b3759d Mon Sep 17 00:00:00 2001 From: sun Date: Wed, 7 Aug 2019 20:03:44 +0800 Subject: [PATCH 1/3] energy changed --- energy/src/energy/clear/energy_init.cpp | 12 ++++++------ energy/src/energy/judge/judge_shoot.cpp | 2 +- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index d5964e7..1b7a974 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -91,9 +91,9 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 5000; gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 1500; - gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 55; + gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 45; gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; - gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20; + gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 10; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5; gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2; @@ -105,10 +105,10 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 500; gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 180; - gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 16; - gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 32; - gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5; - gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20; + gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 10; + gimbal_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50; + gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 0; + gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 30; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index 83ad028..ee9f413 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -30,7 +30,7 @@ void Energy::judgeShootInWorld() { // 此函数用于判断云台坐标系下是否可以发弹 // --------------------------------------------------------------------------------------------------------------------- void Energy::judgeShootInGimbal() { - if (abs(yaw_rotation) < 0.5 && abs(pitch_rotation) < 0.5) { + if (abs(yaw_rotation) < 0.7 && abs(pitch_rotation) < 0.7) { shoot = 4; // is_predicting = false; // is_guessing = true; From 16efe173343c657f39defaa1071aaac824c1de3b Mon Sep 17 00:00:00 2001 From: sun Date: Wed, 7 Aug 2019 20:36:04 +0800 Subject: [PATCH 2/3] energy changed --- energy/src/energy/send/send.cpp | 20 ++++++++++++-------- main.cpp | 6 +++--- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index cc0da07..9f132e0 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -18,10 +18,10 @@ void Energy::sendEnergy() { if (camera_cnt == 1) { sum_yaw += yaw_rotation; sum_pitch += pitch_rotation; - if (ROBOT_ID == 3 || ROBOT_ID == 4 || ROBOT_ID == 8) { + if (ROBOT_ID == 4 || ROBOT_ID == 8) { MINMAX(sum_yaw, -100, 100); MINMAX(sum_yaw, -100, 100); - } else if (ROBOT_ID == 7) { + } else if (ROBOT_ID == 3 || ROBOT_ID == 7) { float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw; float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch; // MINMAX(yaw_I_component, -3, 3); @@ -50,7 +50,7 @@ void Energy::sendEnergy() { // << BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch) << endl; last_yaw = tmp_yaw; last_pitch = tmp_pitch; - if (ROBOT_ID == 7) { + if (ROBOT_ID == 3 || ROBOT_ID == 7) { MINMAX(yaw_rotation, -6, 6); MINMAX(pitch_rotation, -6, 6); } @@ -68,14 +68,18 @@ void Energy::sendEnergy() { double tmp_yaw = yaw_rotation; double tmp_pitch = pitch_rotation; if (mcu_data.mark == 1) { - yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); + yaw_rotation = TRY_SMALL_YAW_AIM_KP * yaw_rotation + TRY_SMALL_YAW_AIM_KI * sum_yaw + + TRY_SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); pitch_rotation = - TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); + TRY_SMALL_PITCH_AIM_KP * pitch_rotation + TRY_SMALL_PITCH_AIM_KI * sum_pitch + + TRY_SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); } else { - yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); - pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); + yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KI * sum_yaw + + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw); + pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KI * sum_pitch + + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); } - if (ROBOT_ID == 7) { + if (ROBOT_ID == 3 || ROBOT_ID == 7) { MINMAX(yaw_rotation, -6, 6); MINMAX(pitch_rotation, -6, 6); } diff --git a/main.cpp b/main.cpp index e008b3c..6675d6a 100644 --- a/main.cpp +++ b/main.cpp @@ -33,7 +33,7 @@ McuData mcu_data = { // 单片机端回传结构体 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 - ENEMY_RED, // 敌方颜色 + ENEMY_BLUE, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 }; @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); - video_chassis = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); + video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); + video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); From 86b37b3f5ea635cf58ab0fc591097526aab252c3 Mon Sep 17 00:00:00 2001 From: sun Date: Wed, 7 Aug 2019 22:59:02 +0800 Subject: [PATCH 3/3] energy changed --- energy/src/energy/get/aim_point_get.cpp | 1 + energy/src/energy/send/send.cpp | 101 ++++++++++++------------ main.cpp | 6 +- 3 files changed, 53 insertions(+), 55 deletions(-) diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp index d382300..68aea4b 100644 --- a/energy/src/energy/get/aim_point_get.cpp +++ b/energy/src/energy/get/aim_point_get.cpp @@ -29,6 +29,7 @@ void Energy::getAimPoint(cv::Point target_point_) { extra_delta_y = 0; } + double dx = -(target_point_.x - 320 - COMPENSATE_YAW - mcu_data.delta_x - manual_delta_x - extra_delta_x); double dy = -(target_point_.y - 240 - COMPENSATE_PITCH - mcu_data.delta_y - manual_delta_y - extra_delta_y); yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI; diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 9f132e0..a0bc804 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -15,56 +15,55 @@ using namespace std; // --------------------------------------------------------------------------------------------------------------------- void Energy::sendEnergy() { if (is_big) { - if (camera_cnt == 1) { - sum_yaw += yaw_rotation; - sum_pitch += pitch_rotation; - if (ROBOT_ID == 4 || ROBOT_ID == 8) { - MINMAX(sum_yaw, -100, 100); - MINMAX(sum_yaw, -100, 100); - } else if (ROBOT_ID == 3 || ROBOT_ID == 7) { - float yaw_I_component = BIG_YAW_AIM_KI * sum_yaw; - float pitch_I_component = BIG_PITCH_AIM_KI * sum_pitch; -// MINMAX(yaw_I_component, -3, 3); -// MINMAX(pitch_I_component, -3, 3); - MINMAX(yaw_I_component, -2, 2); - MINMAX(pitch_I_component, -2, 2); - } - - double tmp_yaw = yaw_rotation; - double tmp_pitch = pitch_rotation; - if (mcu_data.mark == 1) { - yaw_rotation = TRY_BIG_YAW_AIM_KP * yaw_rotation + TRY_BIG_YAW_AIM_KI * sum_yaw + - TRY_BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); - pitch_rotation = TRY_BIG_PITCH_AIM_KP * pitch_rotation + TRY_BIG_PITCH_AIM_KI * sum_pitch + - TRY_BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); - } else { - yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + - BIG_YAW_AIM_KD * (yaw_rotation - last_yaw); - pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch + - BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch); - } - -// cout << "yaw: "<= 4) { - sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0); - }*/ else { + } else { sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0); } diff --git a/main.cpp b/main.cpp index 6675d6a..e008b3c 100644 --- a/main.cpp +++ b/main.cpp @@ -33,7 +33,7 @@ McuData mcu_data = { // 单片机端回传结构体 BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 - ENEMY_BLUE, // 敌方颜色 + ENEMY_RED, // 敌方颜色 0, // 能量机关x轴补偿量 0, // 能量机关y轴补偿量 }; @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_EXPOSURE, ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_EXPOSURE, ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); - video_chassis = new VideoWrapper("/home/sun/桌面/video_8.7/round1-8-5-7-big.avi"); + video_gimbal = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); + video_chassis = new VideoWrapper("/home/sun/桌面/video_8.8/8.7.4-big.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully.");