From b6f3f04b0323d78976e2bb5f886ac41a920a0262 Mon Sep 17 00:00:00 2001 From: sun Date: Thu, 1 Aug 2019 02:36:03 +0800 Subject: [PATCH 1/8] energy changed --- energy/include/energy/energy.h | 1 + energy/src/energy/clear/energy_init.cpp | 2 ++ energy/src/energy/send/send.cpp | 18 +++++++++++++----- others/include/config/setconfig.h | 20 ++++++++++++++++---- 4 files changed, 32 insertions(+), 9 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index a9e25f0..3a6da62 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -85,6 +85,7 @@ private: float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float origin_yaw, origin_pitch;//初始的云台角度设定值 float shoot;//若为2，则要求主控板发弹 + float last_yaw, last_pitch;//PID中微分项 float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项 systime time_start_guess; diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 1d11c6c..5a89d84 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -52,6 +52,8 @@ void Energy::initEnergy() { yaw_rotation = 0; pitch_rotation = 0; shoot = 0; + last_yaw = 0; + last_pitch = 0; sum_yaw = 0; sum_pitch = 0; diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 281dcf1..deece8a 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -8,6 +8,7 @@ using namespace std; +#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) //---------------------------------------------------------------------------------------------------------------------- // 此函数用于发送能量机关数据 @@ -17,13 +18,20 @@ void Energy::sendEnergy() { if (camera_cnt == 1) { sum_yaw += yaw_rotation; sum_pitch += pitch_rotation; - yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw; - pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch; + MINMAX(sum_yaw, -100, 100); + MINMAX(sum_pitch, -100, 100); +// float KP = 4.5; +// if(abs(yaw_rotation)<0.3)KP=0.1; +// else if(abs(yaw_rotation)<0.5)KP=0.1; +// else KP = 0.1; " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl; + yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw + YAW_AIM_KD * (yaw_rotation - last_yaw); + pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch + + PITCH_AIM_KD * (pitch_rotation - last_pitch); } else if (is_chassis) { sum_yaw += yaw_rotation - mcuData.curr_yaw; sum_pitch += pitch_rotation - mcuData.curr_pitch; - yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw; - pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch; + yaw_rotation = YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + YAW_AIM_KI * sum_yaw; + pitch_rotation = PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + PITCH_AIM_KI * sum_pitch; } } @@ -51,7 +59,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) { time_t t = time(nullptr); if (last_time != t) { last_time = t; - cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; + cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl; fps = 0; } fps += 1; diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index e23db56..cf35c9e 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -31,11 +31,23 @@ #ifndef ENERGY_CAMERA_GAIN #define ENERGY_CAMERA_GAIN (30) #endif -#ifndef AIM_KP - #define AIM_KP (6) +#ifndef YAW_AIM_KD + #define YAW_AIM_KD (0) #endif -#ifndef AIM_KI - #define AIM_KI (0.1) +#ifndef YAW_AIM_KP + #define YAW_AIM_KP (6) +#endif +#ifndef YAW_AIM_KI + #define YAW_AIM_KI (0.1) +#endif +#ifndef PITCH_AIM_KD + #define PITCH_AIM_KD (0) +#endif +#ifndef PITCH_AIM_KP + #define PITCH_AIM_KP (6) +#endif +#ifndef PITCH_AIM_KI + #define PITCH_AIM_KI (0.1) #endif #ifndef COMPENSATE_YAW #define COMPENSATE_YAW (5) From f35c97ddd1a4bc1e8a2f348521cb89592ad44e1f Mon Sep 17 00:00:00 2001 From: xinyang Date: Fri, 2 Aug 2019 18:43:48 +0800 Subject: [PATCH 2/8] fix bug. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8325866..e19eb1d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ PROJECT(SJTU-RM-CV) SET(CMAKE_CXX_STANDARD 11) -SET(CMAKE_BUILD_TYPE DEBUG) +SET(CMAKE_BUILD_TYPE RELEASE) SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DPATH=\"\\\"${PROJECT_SOURCE_DIR}\\\"\"") SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -D${CMAKE_SYSTEM_NAME}") From 199feb3ba9bba0ea277759fdb73fdc077e92e230 Mon Sep 17 00:00:00 2001 From: sun Date: Sat, 3 Aug 2019 16:27:21 +0800 Subject: [PATCH 3/8] energy changed --- energy/src/energy/clear/energy_init.cpp | 16 ++++++++-------- energy/src/energy/find/energy_finder.cpp | 13 +++++++++++++ energy/src/energy/get/predict_point_get.cpp | 5 ++++- energy/src/energy/judge/judge_contour.cpp | 6 +++--- energy/src/energy/run.cpp | 1 + energy/src/energy/send/send.cpp | 8 ++++++++ main.cpp | 10 +++++----- 7 files changed, 42 insertions(+), 17 deletions(-) diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 5a89d84..f485825 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -79,10 +79,10 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 120;//home +// gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official -// gimbal_energy_part_param_.GRAY_THRESH = 225; - gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; + gimbal_energy_part_param_.GRAY_THRESH = 180; + gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; @@ -137,13 +137,13 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 38; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 60; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 8; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 32; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 30; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; // gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1.8; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1.5; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index e7e5e78..a397ec0 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -258,15 +258,28 @@ bool Energy::findFlowStrip(const cv::Mat src) { if (!isValidFlowStripContour(flow_strip_contour)) { continue; } + std::vector intersection; RotatedRect cur_rect = minAreaRect(flow_strip_contour); + if (rotatedRectangleIntersection(cur_rect, candidate_flow_strip_fan, intersection) == 0) { continue; } else if (contourArea(intersection) > energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN) { flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour)); // cout << "intersection: " << contourArea(intersection) << '\t' << cur_rect.center << endl; + } else { + continue; } +// Size2f cur_size = cur_rect.size; +// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; +// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; +// float length_width_ratio = length / width;//计算矩形长宽比 +// double cur_contour_area = contourArea(flow_strip_contour); +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; + // RotatedRect cur_rect = minAreaRect(flow_strip_contour); // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; diff --git a/energy/src/energy/get/predict_point_get.cpp b/energy/src/energy/get/predict_point_get.cpp index 53704ff..7024b29 100644 --- a/energy/src/energy/get/predict_point_get.cpp +++ b/energy/src/energy/get/predict_point_get.cpp @@ -17,7 +17,10 @@ using std::vector; void Energy::getPredictPoint(cv::Point target_point) { if (is_big) { if (energy_rotation_direction == 1) rotate(target_point); - if (energy_rotation_direction == -1) rotate(target_point); + if (energy_rotation_direction == -1) { + predict_rad = -25; + rotate(target_point); + } } else if (is_small) predict_point = target_point; } diff --git a/energy/src/energy/judge/judge_contour.cpp b/energy/src/energy/judge/judge_contour.cpp index 69f03c7..60172a5 100644 --- a/energy/src/energy/judge/judge_contour.cpp +++ b/energy/src/energy/judge/judge_contour.cpp @@ -216,7 +216,7 @@ bool Energy::isValidFlowStripContour(const vector &flow_strip_contour length > energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX || width > energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX) { // cout<<"length width fail."<30)cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; return false; //矩形边长不合适 } @@ -225,12 +225,12 @@ bool Energy::isValidFlowStripContour(const vector &flow_strip_contour if (length_width_ratio > energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX || length_width_ratio < energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN) { // cout<<"hw fail."<3.5)cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; return false; //长宽比不合适 } if (cur_contour_area / cur_size.area() < energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN) { -// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// if(cur_contour_area / cur_size.area()>0.5)cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; return false;//轮廓对矩形的面积占有率不合适 } return true; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 85a90c6..424483c 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -92,6 +92,7 @@ void Energy::runBig(cv::Mat &gimbal_src) { changeTarget(); getTargetPolarAngle(); + if (energy_rotation_init) { initRotation(); return; diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index deece8a..c2b992b 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -33,6 +33,14 @@ void Energy::sendEnergy() { yaw_rotation = YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + YAW_AIM_KI * sum_yaw; pitch_rotation = PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + PITCH_AIM_KI * sum_pitch; } + } else if (is_small){ + sum_yaw += yaw_rotation; + sum_pitch += pitch_rotation; + MINMAX(sum_yaw, -100, 100); + MINMAX(sum_pitch, -100, 100); + yaw_rotation = 2.5 * yaw_rotation + 0.08 * sum_yaw + 1.5 * (yaw_rotation - last_yaw); + pitch_rotation = 2.4 * pitch_rotation + 0.07 * sum_pitch + + 1.3 * (pitch_rotation - last_pitch); } if (change_target) { diff --git a/main.cpp b/main.cpp index 7aea291..1ccd85f 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - ARMOR_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/7.27.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/new/5.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/new/5.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -122,8 +122,8 @@ int main(int argc, char *argv[]) { if (!from_camera) extract(gimbal_src, chassis_src); if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 - energy.runBig(gimbal_src, chassis_src); -// energy.runBig(gimbal_src); +// energy.runBig(gimbal_src, chassis_src); + energy.runBig(gimbal_src); } else if (mcuData.state == SMALL_ENERGY_STATE) { if (last_state != SMALL_ENERGY_STATE) { LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); From 977de37b1463eff42ff7c2e78abc5ae58bb6b4cd Mon Sep 17 00:00:00 2001 From: sun Date: Sun, 4 Aug 2019 01:12:51 +0800 Subject: [PATCH 4/8] energy changed --- energy/include/energy/energy.h | 2 + energy/include/energy/param_struct_define.h | 3 + energy/src/energy/clear/energy_init.cpp | 51 ++--- energy/src/energy/find/energy_finder.cpp | 200 ++++++++++++++------ energy/src/energy/judge/judge_contour.cpp | 4 +- energy/src/energy/run.cpp | 12 +- energy/src/energy/send/send.cpp | 21 +- energy/src/energy/show/show.cpp | 18 +- main.cpp | 140 +++++++------- others/include/config/setconfig.h | 38 ++-- 10 files changed, 302 insertions(+), 187 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 3a6da62..45d26b8 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -132,6 +132,7 @@ private: bool findFlowStrip(const cv::Mat src);//寻找图中的流动条 bool findCenterROI(const cv::Mat src);//框取中心R候选区 bool findFlowStripFan(const cv::Mat src);//寻找图中的流动条所在扇叶 + bool findFlowStripWeak(const cv::Mat src);//弱识别寻找图中的流动条 bool isValidFanContour(cv::Mat &src, const vector &fan_contour);//扇叶矩形尺寸要求 bool isValidArmorContour(const vector &armor_contour);//装甲板矩形尺寸要求 @@ -143,6 +144,7 @@ private: void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板 void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R + void showFlowStrip(std::string windows_name, const cv::Mat src);//显示流动条 void showFlowStripFan(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶 void showGuessTarget(std::string windows_name, const cv::Mat src);//显示猜测点位 diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h index 8b2faf9..9e0fa7f 100644 --- a/energy/include/energy/param_struct_define.h +++ b/energy/include/energy/param_struct_define.h @@ -83,6 +83,9 @@ struct EnergyPartParam { long TARGET_CHANGE_DISTANCE_MAX;//目标未更改时，目标装甲板中心与原目标装甲板中心的距离变化最大值 long TWIN_POINT_MAX;//两个点相同时距离最大值 + + long STRIP_ARMOR_DISTANCE_MIN;//流动条中心和目标装甲板中心距离最小值 + long STRIP_ARMOR_DISTANCE_MAX;//流动条中心和目标装甲板中心距离最大值 }; diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index f485825..16e0b22 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -71,7 +71,6 @@ void Energy::initEnergy() { all_target_armor_centers.clear(); while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop(); - } @@ -81,7 +80,7 @@ void Energy::initEnergy() { void Energy::initEnergyPartParam() { // gimbal_energy_part_param_.GRAY_THRESH = 120;//home // gimbal_energy_part_param_.GRAY_THRESH = 200;//official - gimbal_energy_part_param_.GRAY_THRESH = 180; + gimbal_energy_part_param_.GRAY_THRESH = 180;//game gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 180; gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; @@ -100,8 +99,8 @@ void Energy::initEnergyPartParam() { // gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3; // gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.16; - gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; - gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; + 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; @@ -109,42 +108,41 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 100000; - gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 8; - gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 45; - gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 8; - gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 45; - gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 3; + gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MAX = 200; + gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_MIN = 40; + gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MIN = 6; + gimbal_energy_part_param_.CENTER_R_CONTOUR_LENGTH_MAX = 20; + gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MIN = 6; + gimbal_energy_part_param_.CENTER_R_CONTOUR_WIDTH_MAX = 20; + gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MAX = 2; gimbal_energy_part_param_.CENTER_R_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.3; + gimbal_energy_part_param_.CENTER_R_CONTOUR_AREA_RATIO_MIN = 0.6; gimbal_energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN = 10; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 3000; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 1000; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MAX = 2000; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_MIN = 500; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MIN = 60; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_LENGTH_MAX = 100; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_WIDTH_MAX = 52; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 3; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.65; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MAX = 2.8; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_HW_RATIO_MIN = 1.2; + gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MAX = 0.58; gimbal_energy_part_param_.FLOW_STRIP_FAN_CONTOUR_AREA_RATIO_MIN = 0.34; - gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.65; + gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.58; gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.34; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MAX = 0.2; // gimbal_energy_part_param_.FLOW_STRIP_FAN_NON_ZERO_RATE_MIN = 0.08; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 100000; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 30; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MAX = 700; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_MIN = 100; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MIN = 32; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_LENGTH_MAX = 55; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MIN = 4; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_WIDTH_MAX = 20; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 12; -// gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 4; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 1.5; - gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.5; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MAX = 7; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_HW_RATIO_MIN = 3; + gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_AREA_RATIO_MIN = 0.6; gimbal_energy_part_param_.FLOW_STRIP_CONTOUR_INTERSETION_AREA_MIN = 100; gimbal_energy_part_param_.TWIN_ANGEL_MAX = 10; @@ -153,6 +151,9 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.TARGET_CHANGE_DISTANCE_MAX = 20; gimbal_energy_part_param_.TWIN_POINT_MAX = 20; + gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MIN = 28; + gimbal_energy_part_param_.STRIP_ARMOR_DISTANCE_MAX = 52; + chassis_energy_part_param_.GRAY_THRESH = 120;//home diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index a397ec0..c81a34f 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -14,7 +14,7 @@ using std::vector; // 此函数用于寻找图像内所有的大风车扇叶 // --------------------------------------------------------------------------------------------------------------------- int Energy::findFans(const cv::Mat src) { - if (src.empty()){ + if (src.empty()) { if (show_info) cout << "empty!" << endl; return 0; } @@ -62,7 +62,7 @@ int Energy::findFans(const cv::Mat src) { // 此函数用于寻找图像内所有的大风车装甲板模块 // --------------------------------------------------------------------------------------------------------------------- int Energy::findArmors(const cv::Mat src) { - if (src.empty()){ + if (src.empty()) { if (show_info) cout << "empty!" << endl; return 0; } @@ -77,40 +77,44 @@ int Energy::findArmors(const cv::Mat src) { ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); if (show_process)imshow("armor struct", src_bin); -// findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); -// for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 -// { -// unsigned long external_contour_size = armor_contours_external[i].size(); -// for (int j = 0; j < armor_contours.size(); j++) { -// unsigned long all_size = armor_contours[j].size(); -// if (external_contour_size == all_size) { -// swap(armor_contours[j], armor_contours[armor_contours.size() - 1]); -// armor_contours.pop_back(); -// break; -// } -// } -// } + findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 + { + unsigned long external_contour_size = armor_contours_external[i].size(); + for (int j = 0; j < armor_contours.size(); j++) { + unsigned long all_size = armor_contours[j].size(); + if (external_contour_size == all_size) { + swap(armor_contours[j], armor_contours[armor_contours.size() - 1]); + armor_contours.pop_back(); + break; + } + } + } + for (auto &armor_contour : armor_contours) { if (!isValidArmorContour(armor_contour)) { continue; } armors.emplace_back(cv::minAreaRect(armor_contour)); + // RotatedRect cur_rect = minAreaRect(armor_contour); // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// if (length > 5 && width > 5) { -// armors.emplace_back(cv::minAreaRect(armor_contour)); -// cout << "armor area: " << length << '\t' << width << '\t' << cur_rect.center << endl; -// } +// double cur_contour_area = contourArea(armor_contour); +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// cout<(armors.size()); } @@ -120,7 +124,7 @@ int Energy::findArmors(const cv::Mat src) { // 此函数用于寻找图像内大风车中心字母“R” // --------------------------------------------------------------------------------------------------------------------- bool Energy::findCenterR(const cv::Mat src) { - if (src.empty()){ + if (src.empty()) { if (show_info) cout << "empty!" << endl; return false; } @@ -147,22 +151,18 @@ bool Energy::findCenterR(const cv::Mat src) { // Size2f cur_size = cur_rect.size; // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// if (length < 10 || width < 5 || length>19) { -// continue; -// } -// std::vector intersection; -// if (rotatedRectangleIntersection(cur_rect, center_ROI, intersection) != 0 && -// contourArea(intersection) > energy_part_param_.CENTER_R_CONTOUR_INTERSETION_AREA_MIN) { -// centerR = cv::minAreaRect(center_R_contour); -// cout << "center R area: " << length << '\t' << width << '\t' << cur_rect.center << endl; -// cout << "R intersection: " << contourArea(intersection) << endl; -// return true; -// } -// cout << cur_rect.center << endl; +// double cur_contour_area = contourArea(center_R_contour); +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// cout< cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; // double cur_contour_area = contourArea(flow_strip_fan_contour); -// if (length > 40 && width > 30 && length < 110 && width < 100) { -// cout << cur_rect.center< cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// float length_width_ratio = length / width;//计算矩形长宽比 // double cur_contour_area = contourArea(flow_strip_contour); +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; // cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; // cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; // cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; - -// RotatedRect cur_rect = minAreaRect(flow_strip_contour); -// Size2f cur_size = cur_rect.size; -// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; -// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; -// if (length / width > 2.5 && width > 7 && width<40) { -// cout << cur_rect.center << endl; -// flow_strip = cv::minAreaRect(flow_strip_contour); -// cout << "flow strip area: " << length << '\t' << width << endl; -// } -// cout << cur_rect.center << endl; +// cout< candidate_armors = armors; + for (auto &candidate_armor: candidate_armors) { + Point2f vertices[4]; + candidate_armor.size.height *= 1.3; + candidate_armor.size.width *= 1.3; + candidate_armor.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) { + line(src_bin, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 0), 20); + } + } + + cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 + + FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 + if (show_process)imshow("weak struct", src_bin); +// waitKey(0); + + std::vector > flow_strip_contours; + findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); + + for (auto &flow_strip_contour : flow_strip_contours) { + if (!isValidFlowStripContour(flow_strip_contour)) { + continue; + } + + flow_strips.emplace_back(cv::minAreaRect(flow_strip_contour)); + +// RotatedRect cur_rect = minAreaRect(flow_strip_contour); +// Size2f cur_size = cur_rect.size; +// float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; +// float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; +// double cur_contour_area = contourArea(flow_strip_contour); +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// cout << endl; + + } +// cout << "size: " << flow_strips.size() << endl; + + if (flow_strips.empty()) { + if (show_info)cout << "weak flow strip false!" << endl; +// waitKey(0); + return false; + } else { + for (const auto &candidate_flow_strip: flow_strips) { + for (const auto &candidate_armor: armors) { + if (pointDistance(candidate_flow_strip.center, candidate_armor.center) < + energy_part_param_.STRIP_ARMOR_DISTANCE_MIN || + pointDistance(candidate_flow_strip.center, candidate_armor.center) > + energy_part_param_.STRIP_ARMOR_DISTANCE_MAX) { + continue; + } + float angle_armor = candidate_armor.size.width > candidate_armor.size.height ? candidate_armor.angle : + candidate_armor.angle - 90; + float angle_strip = candidate_flow_strip.size.width > candidate_flow_strip.size.height ? + candidate_flow_strip.angle: candidate_flow_strip.angle - 90; + + if (abs(angle_armor - angle_strip) < 60 || abs(angle_armor - angle_strip) > 120) { + continue; + } + target_armor = candidate_armor; + target_point = candidate_armor.center; + flow_strip = candidate_flow_strip; + return true; + } + } + if (show_info)cout << "weak flow strip false!" << endl; +// waitKey(0); + return false; + } +} + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于框取中心R的寻找范围 // --------------------------------------------------------------------------------------------------------------------- diff --git a/energy/src/energy/judge/judge_contour.cpp b/energy/src/energy/judge/judge_contour.cpp index 60172a5..6f43747 100644 --- a/energy/src/energy/judge/judge_contour.cpp +++ b/energy/src/energy/judge/judge_contour.cpp @@ -100,8 +100,8 @@ bool Energy::isValidArmorContour(const vector &armor_contour) { // --------------------------------------------------------------------------------------------------------------------- bool Energy::isValidCenterRContour(const vector ¢er_R_contour) { double cur_contour_area = contourArea(center_R_contour); - if (cur_contour_area > energy_part_param_.ARMOR_CONTOUR_AREA_MAX || - cur_contour_area < energy_part_param_.ARMOR_CONTOUR_AREA_MIN) { + if (cur_contour_area > energy_part_param_.CENTER_R_CONTOUR_AREA_MAX || + cur_contour_area < energy_part_param_.CENTER_R_CONTOUR_AREA_MIN) { //cout<init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); + if (mcuData.state == BIG_ENERGY_STATE) {//大能量机关模式 + if (last_state != BIG_ENERGY_STATE) {//若上一帧不是大能量机关模式，即刚往完成切换，则需要初始化 + LOGM(STR_CTR(WORD_BLUE, "Start Big Energy!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } } + checkReconnect(video_chassis->read(chassis_src)); + energy.setBigEnergyInit(); } - checkReconnect(video_chassis->read(chassis_src)); - energy.setBigEnergyInit(); - } - last_state = mcuData.state;//更新上一帧状态 - ok = checkReconnect(video_gimbal->read(gimbal_src)); - video_chassis->read(chassis_src); + last_state = mcuData.state;//更新上一帧状态 + ok = checkReconnect(video_gimbal->read(gimbal_src)); + video_chassis->read(chassis_src); #ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); #endif #ifdef CHASSIS_FLIP_MODE - flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); + flip(chassis_src, chassis_src, CHASSIS_FLIP_MODE); #endif - if (!from_camera) extract(gimbal_src, chassis_src); - if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 - if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 -// energy.runBig(gimbal_src, chassis_src); - energy.runBig(gimbal_src); - } else if (mcuData.state == SMALL_ENERGY_STATE) { - if (last_state != SMALL_ENERGY_STATE) { - LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); - destroyAllWindows(); - if (from_camera) { - delete video_gimbal; - video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); - if (video_gimbal->init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); + if (!from_camera) extract(gimbal_src, chassis_src); + if (save_video) saveVideos(gimbal_src, chassis_src);//保存视频 + if (show_origin) showOrigin(gimbal_src, chassis_src);//显示原始图像 +// energy.runBig(gimbal_src, chassis_src); + energy.runBig(gimbal_src); + if (show_energy || show_process) waitKey(1); + } else if (mcuData.state == SMALL_ENERGY_STATE) { + if (last_state != SMALL_ENERGY_STATE) { + LOGM(STR_CTR(WORD_GREEN, "Start Small Energy!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } + } + energy.setSmallEnergyInit(); + } + last_state = mcuData.state;//更新上一帧状态 + ok = checkReconnect(video_gimbal->read(gimbal_src)); +#ifdef GIMBAL_FLIP_MODE + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); +#endif + if (!from_camera) extract(gimbal_src); + if (save_video) saveVideos(gimbal_src);//保存视频 + if (show_origin) showOrigin(gimbal_src);//显示原始图像 + energy.runSmall(gimbal_src); + if (show_energy || show_process) waitKey(1); + } else { // 自瞄模式 + if (last_state != ARMOR_STATE) { + LOGM(STR_CTR(WORD_RED, "Start Armor!")); + destroyAllWindows(); + if (from_camera) { + delete video_gimbal; + video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); + if (video_gimbal->init()) { + LOGM("video_gimbal source initialization successfully."); + } else { + LOGW("video_gimbal source unavailable!"); + } } } - energy.setSmallEnergyInit(); - } - last_state = mcuData.state;//更新上一帧状态 - ok = checkReconnect(video_gimbal->read(gimbal_src)); + last_state = mcuData.state; + CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { + if (!checkReconnect(video_gimbal->read(gimbal_src))) continue; + }); #ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); -#endif - if (!from_camera) extract(gimbal_src); - if (save_video) saveVideos(gimbal_src);//保存视频 - if (show_origin) showOrigin(gimbal_src);//显示原始图像 - energy.runSmall(gimbal_src); - } else { // 自瞄模式 - if (last_state != ARMOR_STATE) { - LOGM(STR_CTR(WORD_RED, "Start Armor!")); - destroyAllWindows(); - if (from_camera) { - delete video_gimbal; - video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); - if (video_gimbal->init()) { - LOGM("video_gimbal source initialization successfully."); - } else { - LOGW("video_gimbal source unavailable!"); - } - } - } - last_state = mcuData.state; - CNT_TIME(STR_CTR(WORD_GREEN, "read img"), { - if(!checkReconnect(video_gimbal->read(gimbal_src))) continue; - }); -#ifdef GIMBAL_FLIP_MODE - flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); + flip(gimbal_src, gimbal_src, GIMBAL_FLIP_MODE); #endif // CNT_TIME("something whatever", { if (!from_camera) extract(gimbal_src); if (save_video) saveVideos(gimbal_src); if (show_origin) showOrigin(gimbal_src); // }); - CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), { - armorFinder.run(gimbal_src); - }); - } + CNT_TIME(STR_CTR(WORD_CYAN, "Armor Time"), { + armorFinder.run(gimbal_src); + }); + } // cv::waitKey(0); }); } while (ok); diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index cf35c9e..f831bbd 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -29,25 +29,37 @@ #define ARMOR_CAMERA_GAIN (30) #endif #ifndef ENERGY_CAMERA_GAIN - #define ENERGY_CAMERA_GAIN (30) + #define ENERGY_CAMERA_GAIN (20) #endif -#ifndef YAW_AIM_KD - #define YAW_AIM_KD (0) +#ifndef SMALL_YAW_AIM_KD + #define SMALL_YAW_AIM_KD (1.5) #endif -#ifndef YAW_AIM_KP - #define YAW_AIM_KP (6) +#ifndef SMALL_YAW_AIM_KP + #define SMALL_YAW_AIM_KP (2.5) #endif -#ifndef YAW_AIM_KI - #define YAW_AIM_KI (0.1) +#ifndef SMALL_PITCH_AIM_KD + #define SMALL_PITCH_AIM_KD (1.3) #endif -#ifndef PITCH_AIM_KD - #define PITCH_AIM_KD (0) +#ifndef SMALL_PITCH_AIM_KP + #define SMALL_PITCH_AIM_KP (2.4) #endif -#ifndef PITCH_AIM_KP - #define PITCH_AIM_KP (6) +#ifndef BIG_YAW_AIM_KD + #define BIG_YAW_AIM_KD (0.7) #endif -#ifndef PITCH_AIM_KI - #define PITCH_AIM_KI (0.1) +#ifndef BIG_YAW_AIM_KP + #define BIG_YAW_AIM_KP (4.5) +#endif +#ifndef BIG_YAW_AIM_KI + #define BIG_YAW_AIM_KI (0.1) +#endif +#ifndef BIG_PITCH_AIM_KD + #define BIG_PITCH_AIM_KD (0.7) +#endif +#ifndef BIG_PITCH_AIM_KP + #define BIG_PITCH_AIM_KP (4.5) +#endif +#ifndef BIG_PITCH_AIM_KI + #define BIG_PITCH_AIM_KI (0.1) #endif #ifndef COMPENSATE_YAW #define COMPENSATE_YAW (5) From 851c716153c3c873c81f12fa7ea9c490e674237f Mon Sep 17 00:00:00 2001 From: sun Date: Sun, 4 Aug 2019 01:24:39 +0800 Subject: [PATCH 5/8] energy changed --- energy/include/energy/energy.h | 1 + energy/src/energy/clear/energy_init.cpp | 1 + energy/src/energy/get/predict_point_get.cpp | 8 +++----- main.cpp | 4 ++-- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 45d26b8..ecc2400 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -80,6 +80,7 @@ private: float guess_polar_angle;//猜测的下一个目标装甲板极坐标角度 float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度 float predict_rad;//预测提前角 + float predict_rad_norm;//预测提前角的绝对值 float attack_distance;//步兵与风车平面距离 float center_delta_yaw, center_delta_pitch;//对心时相差的角度 float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index 16e0b22..aaa6327 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -46,6 +46,7 @@ void Energy::initEnergy() { guess_polar_angle = -1000; last_base_angle = -1000; predict_rad = 25; + predict_rad_norm = 25; attack_distance = ATTACK_DISTANCE; center_delta_yaw = 1000; center_delta_pitch = 1000; diff --git a/energy/src/energy/get/predict_point_get.cpp b/energy/src/energy/get/predict_point_get.cpp index 7024b29..4550b29 100644 --- a/energy/src/energy/get/predict_point_get.cpp +++ b/energy/src/energy/get/predict_point_get.cpp @@ -16,11 +16,9 @@ using std::vector; // --------------------------------------------------------------------------------------------------------------------- void Energy::getPredictPoint(cv::Point target_point) { if (is_big) { - if (energy_rotation_direction == 1) rotate(target_point); - if (energy_rotation_direction == -1) { - predict_rad = -25; - rotate(target_point); - } + if (energy_rotation_direction == 1) predict_rad = predict_rad_norm; + else if (energy_rotation_direction == -1) predict_rad = -predict_rad_norm; + rotate(target_point); } else if (is_small) predict_point = target_point; } diff --git a/main.cpp b/main.cpp index 184769f..49b87b4 100644 --- a/main.cpp +++ b/main.cpp @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/new/5.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/new/5.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/new/18.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/new/18.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); From 04c7eea9e0fcb9213cd5617882f282096667cc2b Mon Sep 17 00:00:00 2001 From: sun Date: Sun, 4 Aug 2019 01:44:30 +0800 Subject: [PATCH 6/8] energy changed --- energy/include/energy/energy.h | 1 + energy/src/energy/run.cpp | 11 ++++++++--- energy/src/energy/show/show.cpp | 26 ++++++++++++++++++++++++++ main.cpp | 2 +- 4 files changed, 36 insertions(+), 4 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index ecc2400..14f27cb 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -144,6 +144,7 @@ private: void showFans(std::string windows_name, const cv::Mat src);//显示扇叶 void showArmors(std::string windows_name, const cv::Mat src);//显示装甲板 void showBoth(std::string windows_name, const cv::Mat src);//显示扇叶和装甲板 + void showTarget(std::string windows_name, const cv::Mat src);//显示目标装甲板 void showCenterR(std::string windows_name, const cv::Mat src);//显示风车中心候选区R void showFlowStrip(std::string windows_name, const cv::Mat src);//显示流动条 void showFlowStripFan(std::string windows_name, const cv::Mat src);//显示流动条所在扇叶 diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index ee6b748..0eaa6e2 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -120,9 +120,14 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (show_process)imshow("bin", gimbal_src); if (findArmors(gimbal_src) < 1)return; if (show_energy)showArmors("armor", gimbal_src); - if (!findFlowStripFan(gimbal_src))return; - if (!findTargetInFlowStripFan()) return; - if (!findFlowStrip(gimbal_src))return; + if (!findFlowStripFan(gimbal_src)) { + if (!findFlowStripWeak(gimbal_src))return; + } else { + if (show_energy)showFlowStripFan("strip fan", gimbal_src); + if (!findTargetInFlowStripFan()) return; + if (!findFlowStrip(gimbal_src))return; + } + if(show_energy)showTarget("target", gimbal_src); // getCenter(); // sendEnergy(); diff --git a/energy/src/energy/show/show.cpp b/energy/src/energy/show/show.cpp index c7418f0..23a8205 100644 --- a/energy/src/energy/show/show.cpp +++ b/energy/src/energy/show/show.cpp @@ -98,6 +98,32 @@ void Energy::showBoth(std::string windows_name, const cv::Mat src) { } +//---------------------------------------------------------------------------------------------------------------------- +// 此函数用于显示图像中目标装甲板 +// --------------------------------------------------------------------------------------------------------------------- +void Energy::showTarget(std::string windows_name, const cv::Mat src) { + if (src.empty())return; + static Mat image2show; + + if (src.type() == CV_8UC1) // 黑白图像 + { + cvtColor(src, image2show, COLOR_GRAY2RGB); + + } else if (src.type() == CV_8UC3) //RGB 彩色 + { + image2show = src.clone(); + } + for (const auto &armor : armors) { + if (pointDistance(armor.center, target_point) < energy_part_param_.TWIN_POINT_MAX) { + Point2f vertices[4]; //定义矩形的4个顶点 + armor.points(vertices); //计算矩形的4个顶点 + for (int i = 0; i < 4; i++) + line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(255, 255, 0), 2); + } + } + imshow(windows_name, image2show); +} + //---------------------------------------------------------------------------------------------------------------------- // 此函数用于显示图像中所有可能的风车中心候选区R // --------------------------------------------------------------------------------------------------------------------- diff --git a/main.cpp b/main.cpp index 49b87b4..590ead8 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 From a68a051ce79b44aa3148466f8440ae72e9cf2938 Mon Sep 17 00:00:00 2001 From: xinyang Date: Sun, 4 Aug 2019 08:05:26 +0800 Subject: [PATCH 7/8] fix bug. --- main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/main.cpp b/main.cpp index 590ead8..7963269 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + ARMOR_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 From 886109e3cd2a7f5b4d2d629b0664eb03f310f89b Mon Sep 17 00:00:00 2001 From: sun Date: Sun, 4 Aug 2019 14:02:59 +0800 Subject: [PATCH 8/8] energy changed --- energy/include/energy/energy.h | 1 + energy/src/energy/clear/energy_init.cpp | 15 ++++++++------- energy/src/energy/find/energy_finder.cpp | 15 +++++++-------- energy/src/energy/mark/mark.cpp | 2 +- energy/src/energy/run.cpp | 6 +++++- energy/src/energy/send/send.cpp | 5 ++++- main.cpp | 6 +++--- others/include/config/setconfig.h | 8 ++++---- 8 files changed, 33 insertions(+), 25 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 14f27cb..867a12c 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -66,6 +66,7 @@ private: int send_cnt;//向主控板发送的数据总次数 int camera_cnt;//摄像头数量 + int fans_cnt;//扇叶个数 int last_fans_cnt;//上一帧的扇叶个数 int guess_devide;//刚进入猜测状态时，猜测目标点在极坐标中的分区 int energy_rotation_direction;//风车旋转方向 diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp index aaa6327..80caba9 100644 --- a/energy/src/energy/clear/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -30,6 +30,7 @@ void Energy::initEnergy() { send_cnt = 0; camera_cnt = 1; + fans_cnt = 0; last_fans_cnt = 0; guess_devide = 0; energy_rotation_direction = ANTICLOCKWISE; @@ -86,15 +87,15 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.FAN_GRAY_THRESH = 75; gimbal_energy_part_param_.ARMOR_GRAY_THRESH = 80; - gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 6600; - gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 0; - gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80; - gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 100; + gimbal_energy_part_param_.FAN_CONTOUR_AREA_MAX = 3000; + gimbal_energy_part_param_.FAN_CONTOUR_AREA_MIN = 500; + gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 55; + gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 95; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 20; gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 52; - gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 4; - gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1; - gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.65; + gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MAX = 3.5; + gimbal_energy_part_param_.FAN_CONTOUR_HW_RATIO_MIN = 1.2; + gimbal_energy_part_param_.FAN_CONTOUR_AREA_RATIO_MIN = 0.6; gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.8; gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MIN = 0.48; // gimbal_energy_part_param_.FAN_NON_ZERO_RATE_MAX = 0.3; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index c81a34f..9827d7c 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -39,14 +39,13 @@ int Energy::findFans(const cv::Mat src) { // float length = cur_size.height > cur_size.width ? cur_size.height : cur_size.width; // float width = cur_size.height < cur_size.width ? cur_size.height : cur_size.width; // double cur_contour_area = contourArea(fan_contour); -// double non_zero_rate = nonZeroRateOfRotateRect(src_bin, cur_rect); -// if (length > 60 && width > 20) { -// fans.emplace_back(cv::minAreaRect(fan_contour)); -// cout << cur_rect.center << endl; -// cout << "fan area: " << length << '\t' << width << endl; -// cout << "non zero: " << nonZeroRateOfRotateRect(src_bin, cur_rect) << endl; -// cout << "rate: " << cur_contour_area / cur_size.area() << endl; -// } +// float length_width_ratio = length / width; +// cout << "area: " << cur_contour_area << '\t' << endl; +// cout << "length: " << length << '\t' << "width: " << width << '\t' << cur_rect.center << endl; +// cout << "HW: " << length_width_ratio << '\t' << cur_rect.center << endl; +// cout << "area ratio: " << cur_contour_area / cur_size.area() << '\t' << cur_rect.center << endl; +// cout<= 4) { + if (fans_cnt >= 4) { FILE *fp_delta = fopen(PROJECT_DIR"/Mark/delta.txt", "w"); if (fp_delta) { fprintf(fp_delta, "delta_x: %d, delta_y: %d\n", mcuData.delta_x + manual_delta_x, diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 0eaa6e2..9cf3006 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -89,6 +89,8 @@ void Energy::runBig(cv::Mat &gimbal_src) { if (show_energy)showFlowStrip("strip", gimbal_src); if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); + fans_cnt = findFans(gimbal_src); + if (show_energy)showFans("fans", gimbal_src); // getCenter(); // sendEnergy(); @@ -127,7 +129,9 @@ void Energy::runSmall(cv::Mat &gimbal_src) { if (!findTargetInFlowStripFan()) return; if (!findFlowStrip(gimbal_src))return; } - if(show_energy)showTarget("target", gimbal_src); + if (show_energy)showTarget("target", gimbal_src); + fans_cnt = findFans(gimbal_src); + if (show_energy)showFans("fans", gimbal_src); // getCenter(); // sendEnergy(); diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 36d0476..c140ae8 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -38,11 +38,14 @@ void Energy::sendEnergy() { pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch); } + if (change_target) { sendTarget(serial, yaw_rotation, pitch_rotation, 5, 0); } else if (is_guessing) { sendTarget(serial, yaw_rotation, pitch_rotation, 6, 0); - } else { + } /*else if (fans_cnt >= 4) { + sendTarget(serial, yaw_rotation, pitch_rotation, 7, 0); + }*/ else { sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0); } diff --git a/main.cpp b/main.cpp index 590ead8..0826b70 100644 --- a/main.cpp +++ b/main.cpp @@ -30,7 +30,7 @@ using namespace std; mcu_data mcuData = { // 单片机端回传结构体 0, // 当前云台yaw角 0, // 当前云台pitch角 - SMALL_ENERGY_STATE, // 当前状态，自瞄-大符-小符 + BIG_ENERGY_STATE, // 当前状态，自瞄-大符-小符 0, // 云台角度标记位 1, // 是否启用数字识别 ENEMY_RED, // 敌方颜色 @@ -66,8 +66,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(ARMOR_CAMERA_GAIN, 2/*, "armor"*/); video_chassis = new CameraWrapper(ENERGY_CAMERA_GAIN, 2/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/new/18.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/new/18.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/new/79.avi"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/new/79.avi"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h index f831bbd..7a7c34e 100644 --- a/others/include/config/setconfig.h +++ b/others/include/config/setconfig.h @@ -44,19 +44,19 @@ #define SMALL_PITCH_AIM_KP (2.4) #endif #ifndef BIG_YAW_AIM_KD - #define BIG_YAW_AIM_KD (0.7) + #define BIG_YAW_AIM_KD (1) #endif #ifndef BIG_YAW_AIM_KP - #define BIG_YAW_AIM_KP (4.5) + #define BIG_YAW_AIM_KP (5.5) #endif #ifndef BIG_YAW_AIM_KI #define BIG_YAW_AIM_KI (0.1) #endif #ifndef BIG_PITCH_AIM_KD - #define BIG_PITCH_AIM_KD (0.7) + #define BIG_PITCH_AIM_KD (1) #endif #ifndef BIG_PITCH_AIM_KP - #define BIG_PITCH_AIM_KP (4.5) + #define BIG_PITCH_AIM_KP (5.5) #endif #ifndef BIG_PITCH_AIM_KI #define BIG_PITCH_AIM_KI (0.1)