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!"));