From 27d88c8cee57dc48728e5805013a99ff0f618a7e Mon Sep 17 00:00:00 2001 From: JiatongSun Date: Sat, 27 Apr 2019 16:22:23 +0800 Subject: [PATCH] Auto Aim --- energy/include/energy/energy.h | 1 + energy/src/energy/energy.cpp | 2 ++ energy/src/energy/get/hit_point_get.cpp | 46 ++++++++++++++----------- energy/src/energy/run.cpp | 3 ++ 4 files changed, 32 insertions(+), 20 deletions(-) diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 61a5763..1e6cbc0 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -40,6 +40,7 @@ private: EnergyPartParam energy_part_param_; LiftHeight lift_height_; + bool isSendTarget; int fans_cnt; int armors_cnt; int count; diff --git a/energy/src/energy/energy.cpp b/energy/src/energy/energy.cpp index 360c83a..f4ef66a 100644 --- a/energy/src/energy/energy.cpp +++ b/energy/src/energy/energy.cpp @@ -12,6 +12,8 @@ Energy::Energy(Uart &u):uart(u), src_blue(SRC_HEIGHT, SRC_WIDTH, CV_8UC1), src_red(SRC_HEIGHT, SRC_WIDTH, CV_8UC1) { + isSendTarget = false; + fans_cnt = 0; armors_cnt = 0; cycle_center = Point(0, 0); diff --git a/energy/src/energy/get/hit_point_get.cpp b/energy/src/energy/get/hit_point_get.cpp index 4656af6..cb18052 100644 --- a/energy/src/energy/get/hit_point_get.cpp +++ b/energy/src/energy/get/hit_point_get.cpp @@ -12,75 +12,81 @@ using std::vector; void Energy::getHitPoint(){ int hit_position = 1000; + int limit_angle = 3; + int angle_interval = 60; if(energy_part_rotation==1){ //顺时针 - if(target_armor>=0&&target_armor<60){ + 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 = -60; + hit_position = -1*angle_interval; } - if(target_armor>=60&&target_armor<120){ + if(target_armor>=angle_interval && target_armor(radius); hit_point.y = cycle_center.y; hit_position = 0; } - if(target_armor>=120&&target_armor<=180){ + 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 = 60; + hit_position = angle_interval; } - if(target_armor>=-180&&target_armor<-120){ + 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 = 120; + hit_position = 2*angle_interval; } - if(target_armor>=-120&&target_armor<-60){ + 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>=-60&&target_armor<0){ + 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 = -120; + hit_position = -2 * angle_interval; } + else return; } if(energy_part_rotation==-1){ //逆时针 - if(target_armor>=0&&target_armor<60){ + 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 = 120; + hit_position = 2*angle_interval; } - if(target_armor>=60&&target_armor<120){ + if(target_armor>=angle_interval && target_armor(radius); hit_point.y = cycle_center.y; hit_position = 180; } - if(target_armor>=120&&target_armor<=180){ + 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 = -120; + hit_position = -2*angle_interval; } - if(target_armor>=-180&&target_armor<-120){ + 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 = -60; + hit_position = -1*angle_interval; } - if(target_armor>=-120&&target_armor<-60){ + 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; } - if(target_armor>=-60&&target_armor<0){ + 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 = 60; + hit_position = angle_interval; } + else return; } 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); diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp index dc7a83d..2b5db2d 100644 --- a/energy/src/energy/run.cpp +++ b/energy/src/energy/run.cpp @@ -66,6 +66,9 @@ int Energy::run(cv::Mat &src){ cout << "The hit point position is " << hit_point << endl; // hit_point = cycle_center; + if(!isSendTarget)return 0; + + gimbleRotation(); sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);