From 306afc0ccd9d8a15a4592928104aa0b1d552829e Mon Sep 17 00:00:00 2001 From: JiatongSun Date: Sun, 28 Apr 2019 21:08:30 +0800 Subject: [PATCH] Auto Aim --- energy/include/energy/energy.h | 3 +- .../src/energy/calculate/cycle_calculate.cpp | 4 +- energy/src/energy/calibrate/structing.cpp | 26 ++++-- energy/src/energy/energy.cpp | 1 + energy/src/energy/find/energy_finder.cpp | 12 +-- energy/src/energy/get/gimble_rotation_get.cpp | 2 +- energy/src/energy/get/hit_point_get.cpp | 89 ++++++++++++------- energy/src/energy/get/position_get.cpp | 4 +- energy/src/energy/param_init.cpp | 8 +- energy/src/energy/run.cpp | 19 ++-- others/src/camera/camera_wrapper.cpp | 6 +- 11 files changed, 106 insertions(+), 68 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 1e6cbc0..122f3e2 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -49,6 +49,7 @@ private: double radius; double target_position; double last_target_position; + double last_hit_position; float target_armor; int ally_color_; int energy_part_rotation; @@ -122,7 +123,7 @@ private: void splitBayerBG(cv::Mat &src, cv::Mat &blue, cv::Mat &red); void imagePreprocess(cv::Mat &src); - void StructingElementClose(cv::Mat &src); + void StructingElementClose(cv::Mat &src,int length, int width); void StructingElementErodeDilate(cv::Mat &src); }; diff --git a/energy/src/energy/calculate/cycle_calculate.cpp b/energy/src/energy/calculate/cycle_calculate.cpp index 56d32d6..b8d44d2 100644 --- a/energy/src/energy/calculate/cycle_calculate.cpp +++ b/energy/src/energy/calculate/cycle_calculate.cpp @@ -101,6 +101,6 @@ void Energy::cycleLeastFit() cycle_center.y = static_cast(b / (-2)); radius = sqrt(a * a + b * b - 4 * c) / 2; - cout << "The cycle center is: " << cycle_center << endl; - cout << "The radius is: " << radius << endl; +// cout << "The cycle center is: " << cycle_center << endl; +// cout << "The radius is: " << radius << endl; } diff --git a/energy/src/energy/calibrate/structing.cpp b/energy/src/energy/calibrate/structing.cpp index da8a130..0e16da7 100644 --- a/energy/src/energy/calibrate/structing.cpp +++ b/energy/src/energy/calibrate/structing.cpp @@ -8,19 +8,29 @@ using std::cout; using std::endl; using std::vector; -void Energy::StructingElementClose(cv::Mat &src){ +void Energy::StructingElementClose(cv::Mat &src,int length, int width){ if (src.empty())return; //threshold(src, src, energy_part_param_.CAMERA_GRAY_THRESH, 255, THRESH_BINARY); - Mat element = getStructuringElement(MORPH_RECT, Size(10, 10)); + Mat element = getStructuringElement(MORPH_RECT, Size(length, width)); morphologyEx(src, src, MORPH_CLOSE, element); } void Energy::StructingElementErodeDilate(cv::Mat &src) { cv::Mat src_out, src_out_out; - Mat element_erode = getStructuringElement(MORPH_RECT, Size(4, 4)); - Mat element_dilate = getStructuringElement(MORPH_RECT, Size(20, 20)); - erode(src,src_out, element_erode); - imshow("erode", src_out); - dilate(src_out, src_out_out, element_dilate); - imshow("dilate", src_out_out); + Mat element_erode_1 = getStructuringElement(MORPH_RECT, Size(6, 6)); + Mat element_dilate_1 = getStructuringElement(MORPH_RECT, Size(10, 10)); + Mat element_erode_2 = getStructuringElement(MORPH_RECT, Size(2, 2)); + Mat element_dilate_2 = getStructuringElement(MORPH_RECT, Size(3, 3)); + + dilate(src, src, element_dilate_1); + imshow("dilate_1", src); + + erode(src,src, element_erode_1); + imshow("erode_1", src); + +// dilate(src, src, element_dilate_2); +// imshow("dilate_2", src); +// +// erode(src,src, element_erode_2); +// imshow("erode_2", src); } diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index f4ef66a..18ae357 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -22,6 +22,7 @@ Energy::Energy(Uart &u):uart(u), hit_point = Point(0,0); target_position = -1; last_target_position = -1; + last_hit_position = 20000; target_armor = -1; radius = 0; diff --git a/energy/src/energy/find/energy_finder.cpp b/energy/src/energy/find/energy_finder.cpp index 6838955..798a715 100644 --- a/energy/src/energy/find/energy_finder.cpp +++ b/energy/src/energy/find/energy_finder.cpp @@ -18,8 +18,8 @@ int Energy::findFan(const cv::Mat &src, vector &fans, int &last_fans } std::vector > fan_contours; - StructingElementClose(src_bin); -// imshow("fan struct",src_bin); + StructingElementClose(src_bin,6,6); + imshow("fan struct",src_bin); findContours(src_bin, fan_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE); @@ -36,7 +36,7 @@ int Energy::findFan(const cv::Mat &src, vector &fans, int &last_fans 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; - cout<<"fan area: "<20&&width>20){ // cout< &armors, int &last_ std::vector > armor_contours; std::vector > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 - StructingElementClose(src_bin); -// imshow("armor struct",src_bin); + StructingElementErodeDilate(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); @@ -105,12 +105,12 @@ int Energy::findArmor(const cv::Mat &src, vector &armors, int &last_ // if(length>10&&width>5){ // armors.emplace_back(armor_contour); // cout<<"armor area: "<(armors.size()); return -1; diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp index 22f68f2..6042706 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -24,7 +24,7 @@ void Energy::gimbleRotation(){ // pitch_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE)); yaw_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance)); - pitch_rotation = static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); // yaw_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE)); // pitch_rotation = static_cast(180 / PI * atan2((ATTACK_DISTANCE*tan(9.1*PI/180)-STRETCH*(hit_point.y-cycle_center.y)), ATTACK_DISTANCE)); diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp index cb18052..c5221e2 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -12,41 +12,44 @@ using std::vector; void Energy::getHitPoint(){ int hit_position = 1000; - int limit_angle = 3; + int limit_angle = 6; int angle_interval = 60; if(energy_part_rotation==1){ //顺时针 - if(target_armor>=0 && target_armor<=limit_angle){ + if(target_armor>=15 && target_armor<=15+limit_angle){ hit_point.x = cycle_center.x + static_cast(radius / 2); hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); hit_position = -1*angle_interval; } - if(target_armor>=angle_interval && target_armor(radius); - hit_point.y = cycle_center.y; - hit_position = 0; +// if(target_armor>=angle_interval && target_armor(radius); +// hit_point.y = cycle_center.y; +// hit_position = 0; +// } +// if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ +// hit_point.x = cycle_center.x + static_cast(radius / 2); +// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); +// hit_position = angle_interval; +// } +// if(target_armor>=-180 && target_armor<-180+limit_angle){ +// hit_point.x = cycle_center.x - static_cast(radius / 2); +// hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); +// hit_position = 2*angle_interval; +// } +// if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ +// hit_point.x = cycle_center.x - static_cast(radius); +// hit_point.y = cycle_center.y; +// hit_position = 180; +// } +// if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) { +// hit_point.x = cycle_center.x - static_cast(radius / 2); +// hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); +// hit_position = -2 * angle_interval; +// } + else{ + last_hit_position = 2000; + return; } - if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ - hit_point.x = cycle_center.x + static_cast(radius / 2); - hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); - hit_position = angle_interval; - } - if(target_armor>=-180 && target_armor<-180+limit_angle){ - hit_point.x = cycle_center.x - static_cast(radius / 2); - hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); - hit_position = 2*angle_interval; - } - if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ - hit_point.x = cycle_center.x - static_cast(radius); - hit_point.y = cycle_center.y; - hit_position = 180; - } - if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle) { - hit_point.x = cycle_center.x - static_cast(radius / 2); - hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); - hit_position = -2 * angle_interval; - } - else return; } if(energy_part_rotation==-1){ //逆时针 @@ -80,12 +83,28 @@ void Energy::getHitPoint(){ hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); hit_position = angle_interval; } - else return; + else{ + last_hit_position = 2000; + return; + } } - cout<<"hit position: "<360&&last_hit_position>360){ + isSendTarget = false; + return; + } - isSendTarget = true; + if(fabs(hit_position - last_hit_position) < 30||fabs(hit_position - last_hit_position) > 330 && fabs(hit_position - last_hit_position) <= 360){ + last_hit_position = hit_position; + isSendTarget = false; + return; + } + else{ + last_hit_position = hit_position; + isSendTarget = true; + cout<<"hit position: "<(energy_part_rotation * energy_part_param_.RPM // * energy_part_param_.HIT_TIME * 360 / 60); @@ -93,6 +112,12 @@ void Energy::getHitPoint(){ } bool Energy::changeTarget(){ - return !(pointDistance(target_center, last_target_center) < 25 || fabs(target_position - last_target_position) < 30 - ||fabs(target_position - last_target_position) > 330); + if(fabs(target_position - last_target_position) < 30||fabs(target_position - last_target_position) > 330){ + last_target_position = target_position; + return false; + } + else{ + last_target_position = target_position; + return true; + } } \ No newline at end of file diff --git a/energy/src/energy/get/position_get.cpp b/energy/src/energy/get/position_get.cpp index fc3d51a..5a409d9 100644 --- a/energy/src/energy/get/position_get.cpp +++ b/energy/src/energy/get/position_get.cpp @@ -15,7 +15,7 @@ void Energy::getFanPosition(std::vector &fanPosition, const std::vector(180 / PI * atan2(-1 * (fan.rect.center.y - cycle_center.y), (fan.rect.center.x - cycle_center.x))); fanPosition.push_back(angle); } - cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; +// cout << "fanPosition.size() = " << fanPosition.size() << '\t' << endl; } void Energy::getArmorPosition(std::vector &armorPosition, const std::vector &armors, cv::Point cycle_center, double radius) { @@ -26,7 +26,7 @@ void Energy::getArmorPosition(std::vector &armorPosition, const std::vect armorPosition.push_back(angle); } - cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; +// cout << "armorPosition.size() = " << armorPosition.size() << '\t' << endl; } void Energy::getFirstArmorCenters(vector &armors, std::vector &first_armor_centers) diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index 313d092..77417c6 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -13,7 +13,7 @@ void Energy::initEnergyPartParam() { energy_part_param_.RPM = 10; energy_part_param_.HIT_TIME = 1.14; - energy_part_param_.GRAY_THRESH = 220; + energy_part_param_.GRAY_THRESH = 240; energy_part_param_.SPLIT_GRAY_THRESH = 60; energy_part_param_.FAN_GRAY_THRESH = 75; energy_part_param_.ARMOR_GRAY_THRESH = 80; @@ -30,9 +30,9 @@ void Energy::initEnergyPartParam() { energy_part_param_.ARMOR_CONTOUR_AREA_MAX = 100000; energy_part_param_.ARMOR_CONTOUR_AREA_MIN = 0; energy_part_param_.ARMOR_CONTOUR_LENGTH_MIN = 30; - energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 20; - energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 60; - energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 50; + energy_part_param_.ARMOR_CONTOUR_WIDTH_MIN = 15; + energy_part_param_.ARMOR_CONTOUR_LENGTH_MAX = 50; + energy_part_param_.ARMOR_CONTOUR_WIDTH_MAX = 45; energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MAX = 3; energy_part_param_.ARMOR_CONTOUR_HW_RATIO_MIN = 1; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 5eb2ff7..783b484 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -18,6 +18,7 @@ int Energy::run(cv::Mat &src){ fanPosition.clear(); armorPosition.clear(); gimble_zero_points.clear(); + isSendTarget = false; if(mark==0)return 0; @@ -48,26 +49,26 @@ int Energy::run(cv::Mat &src){ if(armors_cnt != fans_cnt+1) return 0; getAllArmorCenters(); - cout<<"all_armor_centers.size(): "<