From 39f2cd68180202d674750269d8d919cb964bd69e Mon Sep 17 00:00:00 2001 From: sun Date: Mon, 15 Jul 2019 15:43:44 +0800 Subject: [PATCH] energy changed --- energy/include/energy/energy.h | 36 ++++---- energy/src/energy/calibrate/structing.cpp | 27 +++--- energy/src/energy/change/mode_change.cpp | 11 +-- energy/src/energy/change/target_change.cpp | 14 ++-- energy/src/energy/{init => clear}/clear.cpp | 2 +- .../energy/{init => clear}/energy_init.cpp | 82 +++++++++++-------- energy/src/energy/energy.cpp | 18 ++-- energy/src/energy/find/energy_finder.cpp | 30 ++++--- energy/src/energy/get/gimble_rotation_get.cpp | 6 +- energy/src/energy/get/guess_point_get.cpp | 8 +- energy/src/energy/get/polar_angle_get.cpp | 2 +- energy/src/energy/get/predict_point_get.cpp | 5 +- energy/src/energy/judge/judge_mode.cpp | 8 +- energy/src/energy/judge/judge_shoot.cpp | 6 +- energy/src/energy/run.cpp | 35 ++++---- energy/src/energy/tool/tool.cpp | 4 +- main.cpp | 6 +- 17 files changed, 162 insertions(+), 138 deletions(-) rename energy/src/energy/{init => clear}/clear.cpp (99%) rename energy/src/energy/{init => clear}/energy_init.cpp (91%) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 4b431ab..5761530 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -28,7 +28,6 @@ public: ~Energy();//默认析构函数 void run(cv::Mat &gimbal_src, cv::Mat &chassis_src); - void run(cv::Mat &gimbal_src); Serial &serial;//串口 @@ -41,19 +40,20 @@ private: EnergyPartParam gimbal_energy_part_param_;//云台摄像头能量机关的参数设置 EnergyPartParam chassis_energy_part_param_;//底盘摄像头能量机关的参数设置 - bool isMark;//若操作手正在手动标定，则为true - bool isBig;//大符模式为true - bool isSmall;//小符模式为true - bool isGimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 - bool isChassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 - bool isGuessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 - bool isPredicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 + bool is_mark;//若操作手正在手动标定，则为true + bool is_big;//大符模式为true + bool is_small;//小符模式为true + bool is_gimbal;//同时具有底盘和云台摄像头时，处于云台摄像头对心过程 + bool is_chassis;//同时具有底盘和云台摄像头时，处于底盘摄像头击打过程 + bool is_guessing;//当前处于发弹到新目标出现的过程，则为true，此时猜测下一个目标 + bool is_predicting;//当前处于新目标出现到发弹的过程，则为true，此时正常击打 bool energy_mode_init;//正在进行大小符判断 bool energy_rotation_init;//若仍在判断风车旋转方向，则为true bool manual_mark;//若操作手进行过手动标定，则为true bool auto_mark;//云台完成自动对心，则置为true bool shoot;//若为true，则要求主控板发弹 - bool startguessing;//进入猜测状态的标志 + bool start_guess;//进入猜测状态的标志 + bool change_target;//目标切换的标志 uint8_t last_mark;//用于记录上一帧操作手是否进行标定 uint8_t &ally_color;//我方颜色 @@ -80,13 +80,6 @@ private: timeval time_start_guess; - std::vector fans;//图像中所有扇叶 - std::vector armors;//图像中所有可能装甲板（可能存在误识别) - - std::vector all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 - - std::queue recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符 - cv::RotatedRect centerR;//风车中心字母R的可能候选区 cv::RotatedRect flow_strip;//图像中所有流动条（理论上只有一个） cv::RotatedRect flow_strip_fan;//图像中所有流动条所在扇叶（理论上只有一个） @@ -101,6 +94,15 @@ private: cv::Mat src_blue, src_red, src_green;//通道分离中的三个图像通道 + std::vector fans;//图像中所有扇叶 + std::vector armors;//图像中所有可能装甲板（可能存在误识别) + std::vector all_target_armor_centers;//记录全部的装甲板中心，用于风车圆心和半径的计算 + + std::queue recent_target_armor_centers;//记录最近一段时间的装甲板中心，用于判断大符还是小符 + + + + void initEnergy();//能量机关初始化 void initEnergyPartParam();//能量机关参数初始化 void initRotation();//顺逆时针初始化 @@ -146,7 +148,7 @@ private: void writeDownMark();//记录操作手标定的云台初始角度 bool guessTarget();//获得猜测击打点位 - bool changeTarget();//判断目标是否改变 + void changeTarget();//判断目标是否改变 bool getOrigin();//获得云台对心所需角度 void getTargetPolarAngle();//获得目标装甲板极坐标角度 void getPredictPoint(cv::Point target_point);//获取预测点位 diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index dcf55dd..8325b58 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -34,8 +34,6 @@ void Energy::FanStruct(cv::Mat &src) { // imshow("dilate_2", src); // dilate(src, src, element_dilate_3); // imshow("dilate_3", src); -// erode(src,src, element_erode_2); -// imshow("erode_2", src); } @@ -44,19 +42,26 @@ void Energy::FanStruct(cv::Mat &src) { // 此函数对图像进行腐蚀与膨胀操作 // --------------------------------------------------------------------------------------------------------------------- void Energy::ArmorStruct(cv::Mat &src) { - Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(8, 6)); - Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 1)); + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(5, 5)); + Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(2, 2)); Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); - Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(4 , 4)); + Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2 , 2)); + Mat element_dilate_3 = getStructuringElement(MORPH_RECT, Size(3, 3)); + Mat element_erode_3 = getStructuringElement(MORPH_RECT, Size(1 , 1)); - erode(src,src, element_erode_1); -// imshow("erode_1", src); dilate(src, src, element_dilate_1); // imshow("dilate_1", src); - dilate(src, src, element_dilate_2); -// imshow("dilate_2", src); + erode(src,src, element_erode_1); +// imshow("erode_1", src); erode(src,src, element_erode_2); // imshow("erode_2", src); + erode(src,src, element_erode_3); +// imshow("erode_3", src); +// dilate(src, src, element_dilate_2); +// imshow("dilate_2", src); +// dilate(src, src, element_dilate_3); +// imshow("dilate_3", src); + } @@ -84,8 +89,6 @@ void Energy::FlowStripFanStruct(cv::Mat &src) { // imshow("dilate_2", src); // dilate(src, src, element_dilate_3); // imshow("dilate_3", src); -// erode(src,src, element_erode_2); -// imshow("erode_2", src); } @@ -113,8 +116,6 @@ void Energy::FlowStripStruct(cv::Mat &src) { // imshow("dilate_2", src); // dilate(src, src, element_dilate_3); // imshow("dilate_3", src); -// erode(src,src, element_erode_2); -// imshow("erode_2", src); } diff --git a/energy/src/energy/change/mode_change.cpp b/energy/src/energy/change/mode_change.cpp index 61bc571..56d7d4f 100644 --- a/energy/src/energy/change/mode_change.cpp +++ b/energy/src/energy/change/mode_change.cpp @@ -13,8 +13,9 @@ using namespace cv; // 此函数用于从云台摄像头对心状态切换到底盘摄像头击打状态 // --------------------------------------------------------------------------------------------------------------------- void Energy::startChassis(){ - isChassis = true; - isGimbal = false; + is_chassis = true; + is_gimbal = false; + LOGM(STR_CTR(WORD_YELLOW, "Start Chassis!")); } @@ -23,9 +24,9 @@ void Energy::startChassis(){ // 此函数用于判断是否应当继续保持猜测模式 // --------------------------------------------------------------------------------------------------------------------- bool Energy::stayGuessing(){ - if (changeTarget() || isGuessingTimeout()) { - isPredicting = true; - isGuessing = false; + if (change_target || isGuessingTimeout()) { + is_predicting = true; + is_guessing = false; LOGM(STR_CTR(WORD_LIGHT_YELLOW, "Start Predicting!")); return false; } diff --git a/energy/src/energy/change/target_change.cpp b/energy/src/energy/change/target_change.cpp index 7d83e65..46d95e6 100644 --- a/energy/src/energy/change/target_change.cpp +++ b/energy/src/energy/change/target_change.cpp @@ -12,13 +12,11 @@ using namespace cv; //---------------------------------------------------------------------------------------------------------------------- // 此函数用于判断目标是否切换 // --------------------------------------------------------------------------------------------------------------------- -bool Energy::changeTarget(){ - if(pointDistance(target_point,last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX){ - last_target_point = target_point; - return false; - } - else{ - last_target_point= target_point; - return true; +void Energy::changeTarget(){ + if (pointDistance(target_point, last_target_point) < energy_part_param_.TARGET_CHANGE_DISTANCE_MAX) { + change_target = false; + } else { + change_target = true; } + last_target_point = target_point; } \ No newline at end of file diff --git a/energy/src/energy/init/clear.cpp b/energy/src/energy/clear/clear.cpp similarity index 99% rename from energy/src/energy/init/clear.cpp rename to energy/src/energy/clear/clear.cpp index 25d1c06..5aefa1b 100644 --- a/energy/src/energy/init/clear.cpp +++ b/energy/src/energy/clear/clear.cpp @@ -28,4 +28,4 @@ void Energy::initImage(cv::Mat &src){ if (src.type() == CV_8UC3)cvtColor(src, src, COLOR_BGR2GRAY); threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY); if (show_process)imshow("bin", src); -} \ No newline at end of file +} diff --git a/energy/src/energy/init/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp similarity index 91% rename from energy/src/energy/init/energy_init.cpp rename to energy/src/energy/clear/energy_init.cpp index cc6338a..d161b9b 100644 --- a/energy/src/energy/init/energy_init.cpp +++ b/energy/src/energy/clear/energy_init.cpp @@ -14,43 +14,54 @@ using std::vector; // 此函数对能量机关成员变量进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergy() { - isMark = false; - isPredicting = true; - isGuessing = false; + is_mark = false; + is_guessing = false; + is_predicting = true; + energy_mode_init = true; + energy_rotation_init = true; manual_mark = false; auto_mark = false; - energy_mode_init = true; + shoot = false; + start_guess = false; + change_target = false; + + last_mark = 0; + + radius = 0; + + send_cnt = 0; + last_fans_cnt = 0; + guess_devide = 0; + energy_rotation_direction = ANTICLOCKWISE; + clockwise_rotation_init_cnt = 0; + anticlockwise_rotation_init_cnt = 0; + last_mode = -1;//既不是大符也不是小符 + + target_polar_angle = -1000; + last_target_polar_angle = -1000; + guess_polar_angle = -1000; + last_base_angle = -1000; + predict_rad = 20; + attack_distance = ATTACK_DISTANCE; + center_delta_yaw = 1000; + center_delta_pitch = 1000; + yaw_rotation = 0; + pitch_rotation = 0; + origin_yaw = 0; + origin_pitch = 0; + circle_center_point = Point(0, 0); target_point = Point(0, 0); last_target_point = Point(0, 0); guess_point = Point(0, 0); predict_point = Point(0, 0); - target_polar_angle = -1000; - guess_polar_angle = -1000; - last_target_polar_angle = -1000; - radius = 0; - energy_rotation_direction = ANTICLOCKWISE; - attack_distance = ATTACK_DISTANCE; - last_mode = -1;//既不是大符也不是小符 - last_fans_cnt = 0; - send_cnt = 0; - yaw_rotation = 0; - pitch_rotation = 0; - last_mark = 0; - - shoot = false; - guess_devide = 0; - startguessing = false; - predict_rad = 20; fans.clear(); armors.clear(); - all_target_armor_centers.clear(); while(!recent_target_armor_centers.empty())recent_target_armor_centers.pop(); - clockwise_rotation_init_cnt = 0; - anticlockwise_rotation_init_cnt = 0; + } @@ -58,19 +69,21 @@ void Energy::initEnergy() { // 此函数对能量机关参数进行初始化 // --------------------------------------------------------------------------------------------------------------------- void Energy::initEnergyPartParam() { - gimbal_energy_part_param_.GRAY_THRESH = 80; +// gimbal_energy_part_param_.GRAY_THRESH = 80; + gimbal_energy_part_param_.GRAY_THRESH = 225; gimbal_energy_part_param_.SPLIT_GRAY_THRESH = 230; 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 = 56; - gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MAX = 88; - gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MIN = 21; - gimbal_energy_part_param_.FAN_CONTOUR_WIDTH_MAX = 40; + gimbal_energy_part_param_.FAN_CONTOUR_LENGTH_MIN = 80; + 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_MAX = 50; 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_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; @@ -80,8 +93,8 @@ void Energy::initEnergyPartParam() { gimbal_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; 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 = 7; - gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 25; + gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 5; + gimbal_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 20; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; gimbal_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; @@ -128,7 +141,8 @@ void Energy::initEnergyPartParam() { - chassis_energy_part_param_.GRAY_THRESH = 80; +// chassis_energy_part_param_.GRAY_THRESH = 80; + chassis_energy_part_param_.GRAY_THRESH = 225; chassis_energy_part_param_.SPLIT_GRAY_THRESH = 230; chassis_energy_part_param_.FAN_GRAY_THRESH = 75; chassis_energy_part_param_.ARMOR_GRAY_THRESH = 80; @@ -151,8 +165,8 @@ void Energy::initEnergyPartParam() { chassis_energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30; chassis_energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50; - chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15; - chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45; + chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 13; + chassis_energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 33; chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; chassis_energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; chassis_energy_part_param_.ARMOR_CONTOUR_AREA_RATIO_MIN = 0.7; diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 2391d98..f5319f1 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -19,11 +19,10 @@ Energy::Energy(Serial &u, uint8_t &color) : serial(u), ally_color(color), initEnergy(); initEnergyPartParam(); - energy_rotation_init = false; - isBig = false; - isSmall = false; - isGimbal = true; - isChassis = false; + is_big = false; + is_small = false; + is_gimbal = true; + is_chassis = false; } @@ -41,9 +40,8 @@ void Energy::setEnergyInit() { initEnergy(); initEnergyPartParam(); - energy_rotation_init = true; - isBig = false; - isSmall = false; - isGimbal = true; - isChassis = false; + is_big = false; + is_small = false; + is_gimbal = true; + is_chassis = false; } \ No newline at end of file diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index f2d4ae1..ec7e2a5 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -34,12 +34,17 @@ int Energy::findFans(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>20&&width>20){ -// fans.emplace_back(cv::minAreaRect(fan_contour)); -// cout<<"fan area: "< 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; +// } } +// showFans("fan", src_bin); if (fans.size() < last_fans_cnt) { last_fans_cnt = static_cast(fans.size()); return -1;//寻找到的扇叶比上一帧少，说明该帧有误，返回-1 @@ -63,10 +68,10 @@ int Energy::findArmors(const cv::Mat src) { std::vector > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 ArmorStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 -// imshow("armor struct", src_bin); + imshow("armor struct", src_bin); findContours(src_bin, armor_contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE); - findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); +// findContours(src_bin, armor_contours_external, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); // for (int i = 0; i < armor_contours_external.size(); i++)//去除外轮廓 // { @@ -85,12 +90,11 @@ int Energy::findArmors(const cv::Mat src) { 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 > 10 && width > 7) { +// if (length > 5 && width > 5) { // armors.emplace_back(cv::minAreaRect(armor_contour)); // cout << "armor area: " << length << '\t' << width << '\t' << cur_rect.center << endl; // } @@ -98,7 +102,9 @@ int Energy::findArmors(const cv::Mat src) { } // cout<(armors.size()); } @@ -165,7 +171,7 @@ bool Energy::findFlowStripFan(const cv::Mat src) { } std::vector > flow_strip_fan_contours; FlowStripFanStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 -// imshow("flow strip fan struct", src_bin); + imshow("flow strip fan struct", src_bin); findContours(src_bin, flow_strip_fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); @@ -208,7 +214,7 @@ bool Energy::findFlowStrip(const cv::Mat src) { cvtColor(src_bin, src_bin, CV_BGR2GRAY);//若读取三通道视频文件，需转换为单通道 } FlowStripStruct(src_bin);//图像膨胀，防止图像断开并更方便寻找 - imshow("flow strip struct", src_bin); +// imshow("flow strip struct", src_bin); std::vector > flow_strip_contours; findContours(src_bin, flow_strip_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index 10993bd..6ec5dd5 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -19,17 +19,17 @@ void Energy::changeMark() { last_mark = mcuData.mark; origin_yaw = mcuData.curr_yaw; origin_pitch = mcuData.curr_pitch; - isMark = false; + is_mark = false; manual_mark = true; // LOGM(STR_CTR(WORD_LIGHT_YELLOW, "IsMark")); } else if (mcuData.mark == 1) {//正在标定 last_mark = mcuData.mark; - isMark = true; + is_mark = true; // LOGM(STR_CTR(WORD_BLUE,"Marking...")); } else {//未在标定 last_mark = mcuData.mark; - isMark = false; + is_mark = false; } //cout<<"mark: "< all_fan_angles; + vector all_fan_angles; float angle; for (const auto &fan : fans) { angle = static_cast(180 / PI * atan2(-1 * (fan.center.y - circle_center_point.y), @@ -33,7 +33,7 @@ bool Energy::guessTarget() { float max_angle = all_fan_angles.at(all_fan_angles.size() - 1); float base_angle = min_angle; while (base_angle > 72)base_angle -= 72; - if (startguessing) { + if (start_guess) { int i = 0; for (i = 1; i < all_fan_angles.size(); ++i) { if (abs(min_angle + 72 * i - all_fan_angles.at(i)) > energy_part_param_.TWIN_ANGEL_MAX) { @@ -41,8 +41,8 @@ bool Energy::guessTarget() { break; } } - if (i == all_fan_angles.size()) guess_polar_angle = max_angle + 72; - startguessing = false; + if (i == all_fan_angles.size()) guess_polar_angle = (max_angle + 72); + start_guess = false; guess_devide = devide(guess_polar_angle); } else if (abs(base_angle - last_base_angle) > 20) { if (energy_rotation_direction == CLOCKWISE)guess_devide = (guess_devide + 4) % 5; diff --git a/energy/src/energy/get/polar_angle_get.cpp b/energy/src/energy/get/polar_angle_get.cpp index c875585..644d492 100644 --- a/energy/src/energy/get/polar_angle_get.cpp +++ b/energy/src/energy/get/polar_angle_get.cpp @@ -28,7 +28,7 @@ void Energy::getAllTargetArmorCenters() { // 此函数用于存储近30帧图像中所有装甲板的中心坐标，用于判断小符和大符 // --------------------------------------------------------------------------------------------------------------------- void Energy::getRecentTargetArmorCenters() { - if (changeTarget()) { + if (change_target) { while (!recent_target_armor_centers.empty())recent_target_armor_centers.pop();//目标切换时清空 return; } diff --git a/energy/src/energy/get/predict_point_get.cpp b/energy/src/energy/get/predict_point_get.cpp index 54a84ac..53704ff 100644 --- a/energy/src/energy/get/predict_point_get.cpp +++ b/energy/src/energy/get/predict_point_get.cpp @@ -15,10 +15,9 @@ using std::vector; // 此函数获取预测点坐标 // --------------------------------------------------------------------------------------------------------------------- void Energy::getPredictPoint(cv::Point target_point) { - if (isBig) { + if (is_big) { if (energy_rotation_direction == 1) rotate(target_point); if (energy_rotation_direction == -1) rotate(target_point); - } else if (isSmall) predict_point = target_point; - + } else if (is_small) predict_point = target_point; } diff --git a/energy/src/energy/judge/judge_mode.cpp b/energy/src/energy/judge/judge_mode.cpp index b439ac2..1eb5ed4 100644 --- a/energy/src/energy/judge/judge_mode.cpp +++ b/energy/src/energy/judge/judge_mode.cpp @@ -19,16 +19,16 @@ void Energy::JudgeMode() { } else { if (abs(recent_target_armor_centers.back() - recent_target_armor_centers.front()) > 10) { if(last_mode!=BIG){ - isBig = true; - isSmall = false; + is_big = true; + is_small = false; energy_mode_init = false; LOGM(STR_CTR(WORD_CYAN,"start big!")); } last_mode = BIG; } else { if(last_mode!=SMALL){ - isBig = false; - isSmall = true; + is_big = false; + is_small = true; energy_mode_init = false; LOGM(STR_CTR(WORD_GREEN,"start small!")); } diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp index 796fb74..d456af4 100644 --- a/energy/src/energy/judge/judge_shoot.cpp +++ b/energy/src/energy/judge/judge_shoot.cpp @@ -16,9 +16,9 @@ using namespace cv; void Energy::judgeShoot(){ if (abs(yaw_rotation - mcuData.curr_yaw) < 0.3 && fabs(pitch_rotation - mcuData.curr_pitch) < 0.3) { shoot = true; - isPredicting = false; - isGuessing = true; - startguessing = true; + is_predicting = false; + is_guessing = true; + start_guess = true; gettimeofday(&time_start_guess, NULL); LOGM(STR_CTR(WORD_LIGHT_RED, "Start Guessing!")); } else diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index c15f05c..28c6eb6 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -15,12 +15,13 @@ using namespace cv; void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { if (chassis_src.empty()) run(gimbal_src);//仅拥有云台摄像头则调用单摄像头的run函数 - else if (isGimbal) { - energy_part_param_ = gimbal_energy_part_param_; + else if (is_gimbal) { + energy_part_param_ = chassis_energy_part_param_; clearAll(); initImage(gimbal_src); if (findArmors(gimbal_src) < 1)return; + if (show_energy)showArmors("armor", gimbal_src); if (!findFlowStripFan(gimbal_src))return; if (!findTargetInFlowStripFan()) return; if (!findCenterROI(gimbal_src))return; @@ -32,36 +33,39 @@ void Energy::run(cv::Mat &gimbal_src, cv::Mat &chassis_src) { startChassis(); initEnergy(); destroyAllWindows(); - } else if (isChassis) { - energy_part_param_ = gimbal_energy_part_param_; + } else if (is_chassis) { + energy_part_param_ = chassis_energy_part_param_; clearAll(); initImage(chassis_src); changeMark(); - if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch + if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch if (findArmors(chassis_src) < 1)return; - if (!findFlowStripFan(chassis_src))return;showFlowStripFan("flow strip fan", chassis_src); + if (show_energy)showArmors("armor", chassis_src); + if (!findFlowStripFan(chassis_src))return; + showFlowStripFan("flow strip fan", chassis_src); if (!findTargetInFlowStripFan()) return; if (!findCenterROI(chassis_src))return; if (show_energy)showFlowStripFan("strip", chassis_src); if (!findCenterR(chassis_src))return; if (show_energy)showCenterR("R", chassis_src); - getTargetPolarAngle(); + changeTarget(); JudgeMode(); if (energy_mode_init)return; - if (isBig && energy_rotation_init) { + if (is_big && energy_rotation_init) { initRotation(); return; } - if (isPredicting) { + if (is_predicting) { getPredictPoint(target_point); gimbalRotation(); judgeShoot(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else if (isGuessing && stayGuessing()) { + } else if (is_guessing && stayGuessing()) { findFans(chassis_src); + if (show_energy)showFans("fans", chassis_src); if (save_mark)writeDownMark(); if (!guessTarget()) return; if (show_energy)showGuessTarget("guess", chassis_src); @@ -82,31 +86,34 @@ void Energy::run(cv::Mat &gimbal_src) { initImage(gimbal_src); changeMark(); - if (isMark)return;//操作手强制手动标定origin_yaw和origin_pitch + if (is_mark)return;//操作手强制手动标定origin_yaw和origin_pitch 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 (!findCenterROI(gimbal_src))return; if (show_energy)showFlowStripFan("strip", gimbal_src); if (!findCenterR(gimbal_src))return; if (show_energy)showCenterR("R", gimbal_src); + changeTarget(); getTargetPolarAngle(); JudgeMode(); if (energy_mode_init)return; if (!getOrigin())return; - if (isBig & energy_rotation_init) { + if (is_big && energy_rotation_init) { initRotation(); return; } - if (isPredicting) { + if (is_predicting) { getPredictPoint(target_point); gimbalRotation(); judgeShoot(); sendTarget(serial, yaw_rotation, pitch_rotation, shoot); - } else if (isGuessing && stayGuessing()) { + } else if (is_guessing && stayGuessing()) { findFans(gimbal_src); + if (show_energy)showFans("fans", gimbal_src); if (save_mark)writeDownMark(); guessTarget(); if (show_energy)showGuessTarget("guess", gimbal_src); diff --git a/energy/src/energy/tool/tool.cpp b/energy/src/energy/tool/tool.cpp index 792b5cd..bd6727b 100644 --- a/energy/src/energy/tool/tool.cpp +++ b/energy/src/energy/tool/tool.cpp @@ -68,9 +68,7 @@ double Energy::pointDistance(cv::Point point_1, cv::Point point_2) { // --------------------------------------------------------------------------------------------------------------------- int Energy::devide(float angle) { if (angle < 0)angle += 360;//若angle小于0，说明当前角度范围为-180°~180° - int i = 0; - while (angle - 72 * i > 72)i++; - return i; + return angle / 72.0; } diff --git a/main.cpp b/main.cpp index f8224bc..7f0c3a4 100644 --- a/main.cpp +++ b/main.cpp @@ -64,8 +64,8 @@ int main(int argc, char *argv[]) { video_gimbal = new CameraWrapper(0/*, "armor"*/); video_chassis = new CameraWrapper(1/*, "energy"*/); } else { - video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/147.avi"); - video_chassis = new VideoWrapper("/home/sun/项目/energy_video/147.avi"); + video_gimbal = new VideoWrapper("/home/sun/项目/energy_video/大能量机关（蓝+开灯）.mov"); + video_chassis = new VideoWrapper("/home/sun/项目/energy_video/大能量机关（蓝+开灯）.mov"); } if (video_gimbal->init()) { LOGM("video_gimbal source initialization successfully."); @@ -128,7 +128,7 @@ int main(int argc, char *argv[]) { armorFinder.run(gimbal_src); }); } - cv::waitKey(0); +// cv::waitKey(0); }); } while (ok); delete video_gimbal;