From 9cfd26cc23c6d78fd86c0f96653d857950c4084e Mon Sep 17 00:00:00 2001 From: JiatongSun Date: Sat, 27 Apr 2019 15:24:59 +0800 Subject: [PATCH] Auto Aim --- energy/include/energy/constant.h | 8 +-- energy/src/energy/get/hit_point_get.cpp | 85 +++++++++++++++++++++---- energy/src/energy/param_init.cpp | 2 +- energy/src/energy/run.cpp | 28 ++++---- main.cpp | 3 + 5 files changed, 95 insertions(+), 31 deletions(-) diff --git a/energy/include/energy/constant.h b/energy/include/energy/constant.h index 78dc089..cf78798 100644 --- a/energy/include/energy/constant.h +++ b/energy/include/energy/constant.h @@ -19,10 +19,10 @@ const float ATTACK_DISTANCE = 770;//cm const double WHOLE_FAN = 80;//cm const double ARMOR_CENTER_TO_CYCLE_CENTER = 75;//cm //const double ARMOR_CENTER_TO_CYCLE_CENTER = 71;//cm -const int EXTRACT_POINT_X = 200; -const int EXTRACT_POINT_Y = 20; -const int EXTRACT_WIDTH = 240; -const int EXTRACT_HEIGHT = 180; +const int EXTRACT_POINT_X = 120; +const int EXTRACT_POINT_Y = 0; +const int EXTRACT_WIDTH = 400; +const int EXTRACT_HEIGHT = 300; //以摄像头正方向位y轴 const int GM_L = 14;//云台摄像头z方向 diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp index 08886ac..4656af6 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -11,18 +11,79 @@ using std::endl; using std::vector; void Energy::getHitPoint(){ -// if(!changeTarget()){ -// //last_target_center = target_center; -// //last_target_position = target_position; -// return; -// } -// else { - double rad = static_cast(energy_part_rotation * energy_part_param_.RPM - * energy_part_param_.HIT_TIME * 360 / 60); - rotate(rad, radius, cycle_center, target_center, hit_point); - //last_target_center = target_center; - //last_target_position = target_position; -// } + int hit_position = 1000; + + if(energy_part_rotation==1){ //顺时针 + if(target_armor>=0&&target_armor<60){ + hit_point.x = cycle_center.x + static_cast(radius / 2); + hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); + hit_position = -60; + } + if(target_armor>=60&&target_armor<120){ + hit_point.x = cycle_center.x + static_cast(radius); + hit_point.y = cycle_center.y; + hit_position = 0; + } + if(target_armor>=120&&target_armor<=180){ + hit_point.x = cycle_center.x + static_cast(radius / 2); + hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); + hit_position = 60; + } + if(target_armor>=-180&&target_armor<-120){ + hit_point.x = cycle_center.x - static_cast(radius / 2); + hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); + hit_position = 120; + } + if(target_armor>=-120&&target_armor<-60){ + hit_point.x = cycle_center.x - static_cast(radius); + hit_point.y = cycle_center.y; + hit_position = 180; + } + if(target_armor>=-60&&target_armor<0){ + hit_point.x = cycle_center.x - static_cast(radius / 2); + hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); + hit_position = -120; + } + } + + if(energy_part_rotation==-1){ //逆时针 + if(target_armor>=0&&target_armor<60){ + hit_point.x = cycle_center.x - static_cast(radius / 2); + hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); + hit_position = 120; + } + if(target_armor>=60&&target_armor<120){ + hit_point.x = cycle_center.x - static_cast(radius); + hit_point.y = cycle_center.y; + hit_position = 180; + } + if(target_armor>=120&&target_armor<=180){ + hit_point.x = cycle_center.x - static_cast(radius / 2); + hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); + hit_position = -120; + } + if(target_armor>=-180&&target_armor<-120){ + hit_point.x = cycle_center.x + static_cast(radius / 2); + hit_point.y = cycle_center.y + static_cast(radius * sqrt(3) / 2); + hit_position = -60; + } + if(target_armor>=-120&&target_armor<-60){ + hit_point.x = cycle_center.x + static_cast(radius); + hit_point.y = cycle_center.y; + hit_position = 0; + } + if(target_armor>=-60&&target_armor<0){ + hit_point.x = cycle_center.x + static_cast(radius / 2); + hit_point.y = cycle_center.y - static_cast(radius * sqrt(3) / 2); + hit_position = 60; + } + } + + cout<<"hit position: "<(energy_part_rotation * energy_part_param_.RPM +// * energy_part_param_.HIT_TIME * 360 / 60); +// rotate(rad, radius, cycle_center, target_center, hit_point); } bool Energy::changeTarget(){ diff --git a/energy/src/energy/param_init.cpp b/energy/src/energy/param_init.cpp index 55240d2..b5b303e 100644 --- a/energy/src/energy/param_init.cpp +++ b/energy/src/energy/param_init.cpp @@ -14,7 +14,7 @@ void Energy::initEnergyPartParam() { energy_part_param_.HIT_TIME = 1.14; energy_part_param_.GRAY_THRESH = 240; - energy_part_param_.SPLIT_GRAY_THRESH = 80; + energy_part_param_.SPLIT_GRAY_THRESH = 60; energy_part_param_.FAN_GRAY_THRESH = 75; energy_part_param_.ARMOR_GRAY_THRESH = 80; diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index 8c57071..dc7a83d 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -19,13 +19,13 @@ int Energy::run(cv::Mat &src){ armorPosition.clear(); gimble_zero_points.clear(); - if(mark==0)return 0; +// if(mark==0)return 0; // if(all_armor_centers.size()>200)all_armor_centers.clear(); // if(first_armor_centers.size()>200)first_armor_centers.clear(); // cout<<"first_armor_centers.size(): "<0)hit_point = gimble_zero_points.at(0).rect.center; -*/ - - getAllArmorCenters(); cout<<"all_armor_centers.size(): "<0)hit_point = gimble_zero_points.at(0).rect.center; +*/ + + diff --git a/main.cpp b/main.cpp index 0765372..89595da 100644 --- a/main.cpp +++ b/main.cpp @@ -73,6 +73,9 @@ int main(int argc, char *argv[]) imshow("armor src", armor_src); } if(state == ENERGY_STATE){ + if(from_camera==0){ + energy.extract(energy_src); + } energy.run(energy_src); }else{ armorFinder.run(armor_src);