From 306afc0ccd9d8a15a4592928104aa0b1d552829e Mon Sep 17 00:00:00 2001 From: JiatongSun Date: Sun, 28 Apr 2019 21:08:30 +0800 Subject: [PATCH 1/2] 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(): "< Date: Tue, 7 May 2019 09:33:28 +0800 Subject: [PATCH 2/2] Auto Aim --- energy/include/energy/energy.h | 2 + .../src/energy/calculate/cycle_calculate.cpp | 2 +- energy/src/energy/calibrate/structing.cpp | 27 ++-- energy/src/energy/energy.cpp | 3 + energy/src/energy/find/energy_finder.cpp | 1 + energy/src/energy/get/gimble_rotation_get.cpp | 23 +++- energy/src/energy/get/hit_point_get.cpp | 127 ++++++++++++------ energy/src/energy/run.cpp | 13 +- main.cpp | 113 ++++++++-------- 9 files changed, 191 insertions(+), 120 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 122f3e2..71a5455 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -60,6 +60,8 @@ private: double phi;//电机yaw轴应旋转的角度 float yaw_rotation; float pitch_rotation; + int position_mode; + int last_position_mode; int isLeftVertexFound, isTopVertexFound, isRightVertexFound, isBottomVertexFound; std::vector fans; diff --git a/energy/src/energy/calculate/cycle_calculate.cpp b/energy/src/energy/calculate/cycle_calculate.cpp index b8d44d2..7593555 100644 --- a/energy/src/energy/calculate/cycle_calculate.cpp +++ b/energy/src/energy/calculate/cycle_calculate.cpp @@ -58,7 +58,7 @@ void Energy::cycleLeastFit() radius = 0.0f; if (all_armor_centers.size() < 3) { - cout<<"Cannot calculate a circle"< &armors, int &last_ std::vector > armor_contours_external;//用总轮廓减去外轮廓，只保留内轮廓，除去流动条的影响。 StructingElementErodeDilate(src_bin); +// StructingElementClose(src_bin,10,10); imshow("armor struct",src_bin); findContours(src_bin, armor_contours, CV_RETR_LIST, 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 6042706..977648c 100644 --- a/energy/src/energy/get/gimble_rotation_get.cpp +++ b/energy/src/energy/get/gimble_rotation_get.cpp @@ -24,7 +24,28 @@ 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 = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + if(position_mode == 1){ + pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + if(position_mode == 2){ + pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + if(position_mode == 3){ + pitch_rotation = 4.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + if(position_mode == 4){ + pitch_rotation = 4.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + if(position_mode == 5){ + pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } + if(position_mode == 6){ + pitch_rotation = 5.5+static_cast(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance)); + } +// else{ +// 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 c5221e2..552c5a1 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -16,38 +16,47 @@ void Energy::getHitPoint(){ int angle_interval = 60; if(energy_part_rotation==1){ //顺时针 - if(target_armor>=15 && target_armor<=15+limit_angle){ + if(target_armor>=0 && target_armor<=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; + position_mode = 1; +// cout<<"666"<<'\t'<=angle_interval && target_armor(radius); + hit_point.y = cycle_center.y; + hit_position = 0; + position_mode = 2; + } + else 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; + position_mode = 3; + } + else 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; + position_mode = 4; + } + else 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; + position_mode = 5; + } + else 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; + position_mode = 6; } -// 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; +// last_hit_position = 2000; + position_mode = 0; return; } } @@ -57,54 +66,84 @@ void Energy::getHitPoint(){ 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; + position_mode = 1; } - if(target_armor>=angle_interval && target_armor=angle_interval && target_armor(radius); hit_point.y = cycle_center.y; hit_position = 180; + position_mode = 2; } - if(target_armor>=angle_interval*2 && target_armor<=angle_interval*2+limit_angle){ + else 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 = -2*angle_interval; + position_mode = 3; } - if(target_armor>=-180 && target_armor<-180+limit_angle){ + else 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 = -1*angle_interval; + position_mode = 4; } - if(target_armor>=-2*angle_interval&&target_armor<-2*angle_interval+limit_angle){ + else 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 = 0; + position_mode = 5; } - if(target_armor>=-1*angle_interval&&target_armor<-1*angle_interval+limit_angle){ + else 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 = angle_interval; + position_mode = 6; } else{ - last_hit_position = 2000; +// last_hit_position = 2000; + position_mode = 0; return; } } - if(hit_position>360&&last_hit_position>360){ - isSendTarget = false; - return; - } - 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; +// cout<<"last: "<360&&last_hit_position>360){ +// isSendTarget = false; +// return; +// } + +// if(fabs(hit_position - last_hit_position) < 15||fabs(hit_position - last_hit_position) > 345 && 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); diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 783b484..9bae15a 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -12,7 +12,7 @@ extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch; extern int mark; int Energy::run(cv::Mat &src){ -// imshow("src",src); + imshow("src",src); fans.clear(); armors.clear(); fanPosition.clear(); @@ -52,14 +52,14 @@ int Energy::run(cv::Mat &src){ // cout<<"all_armor_centers.size(): "<> from_camera; } - WrapperHead *video_armor; + WrapperHead *video_armor; WrapperHead *video_energy; - if (from_camera) { - video_armor = new CameraWrapper(0); + if(from_camera) { +// video_armor = new CameraWrapper(0); video_energy = new CameraWrapper(1); - } else { - video_armor = new VideoWrapper("/home/xinyang/Desktop/Video.mp4"); - video_energy = new VideoWrapper("/home/xinyang/Desktop/Video.mp4"); - } - if (video_armor->init()) { - cout << "Video source initialization successfully." << endl; + }else { + video_armor = new VideoWrapper("r_l_640.avi"); + video_energy = new VideoWrapper("r_l_640.avi"); } + if (video_energy->init()) { + cout << "Video source initialization successfully." << endl; + } - Mat energy_src, armor_src; - ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/"); + Mat energy_src, armor_src; + + ArmorFinder armorFinder(ENEMY_BLUE, uart, PROJECT_DIR"/tools/para/"); Energy energy(uart); energy.setAllyColor(ally_color); energy.setRotation(energy_part_rotation); - bool ok = true; + bool ok=true; - while (ok) { - CNT_TIME(WORD_LIGHT_CYAN, "Total", { + while (ok){ +// CNT_TIME(WORD_LIGHT_CYAN, "Total", { +// ok = video_energy->read(energy_src) && video_energy->read(armor_src); + ok = video_energy->read(energy_src); + if (show_origin) { + imshow("enery src", energy_src); + imshow("armor src", armor_src); + } if (state == ENERGY_STATE) { - ok = video_energy->read(energy_src); - if (show_origin) { - imshow("energy src", energy_src); - } if (from_camera == 0) { energy.extract(energy_src); } energy.run(energy_src); } else { - ok = video_armor->read(armor_src); - if (show_origin) { - imshow("armor src", armor_src); - } - CNT_TIME(WORD_LIGHT_BLUE, "Armor Time", { - armorFinder.run(armor_src); - }); +// CNT_TIME(WORD_LIGHT_BLUE, "Armor Time", { +// armorFinder.run(armor_src); +// }); } if (waitKey(1) == 'q') { flag = false; break; } - }); - } - delete video_armor; - cout << "Program fails. Restarting" << endl; - } +// }); + } + delete video_energy; + cout << "Program fails. Restarting" << endl; + } - return 0; + return 0; } -#define RECEIVE_LOG_LEVEL LOG_NOTHING +#define RECEIVE_LOG_LEVEL LOG_MSG -void uartReceive(Uart *uart) { +void uartReceive(Uart* uart){ char buffer[100]; - int cnt = 0; - while (true) { + int cnt=0; + while(true){ char data; - while ((data = uart->receive()) != '\n') { + while((data=uart->receive()) != '\n'){ buffer[cnt++] = data; - if (cnt >= 100) { + if(cnt >= 100){ LOG(RECEIVE_LOG_LEVEL, "data receive over flow!"); - cnt = 0; + cnt = 0; } } - if (cnt == 10) { - if (buffer[8] == 'e') { + if(cnt == 10){ + if(buffer[8] == 'e'){ state = ENERGY_STATE; - LOG(RECEIVE_LOG_LEVEL, "Energy state"); - } else if (buffer[8] == 'a') { +// LOG(RECEIVE_LOG_LEVEL, "Energy state"); + }else if(buffer[8] == 'a'){ state = ARMOR_STATE; - LOG(RECEIVE_LOG_LEVEL, "Armor state"); +// LOG(RECEIVE_LOG_LEVEL, "Armor state"); } memcpy(&curr_yaw, buffer, 4); - memcpy(&curr_pitch, buffer + 4, 4); - LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); - if (buffer[9] == 1) { - if (mark == 0) { + memcpy(&curr_pitch, buffer+4, 4); +// LOG(RECEIVE_LOG_LEVEL, "Get yaw:%f pitch:%f", curr_yaw, curr_pitch); + if(buffer[9] == 1){ + if(mark == 0){ mark = 1; mark_yaw = curr_yaw; mark_pitch = curr_pitch; } - LOG(RECEIVE_LOG_LEVEL, "Marked"); +// LOG(RECEIVE_LOG_LEVEL, "Marked"); } } cnt = 0;