From 106fbc871eaa5eba18cf1fc7ca3b735eb8bae1db Mon Sep 17 00:00:00 2001
From: JiatongSun <SJT_sjtu1998@163.com>
Date: Tue, 16 Apr 2019 00:08:25 +0800
Subject: [PATCH] Auto Aim

---
 energy/include/energy/energy.h                |  1 +
 energy/include/energy/param_struct_define.h   | 10 ++++++++++
 energy/src/energy/get/gimble_rotation_get.cpp |  4 ++--
 energy/src/energy/param_init.cpp              |  9 ++++++++-
 energy/src/energy/run.cpp                     | 14 +++++++++-----
 5 files changed, 30 insertions(+), 8 deletions(-)

diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h
index 08850c6..61a5763 100644
--- a/energy/include/energy/energy.h
+++ b/energy/include/energy/energy.h
@@ -39,6 +39,7 @@ public:
 private:
 
 	EnergyPartParam energy_part_param_;
+	LiftHeight lift_height_;
 	int fans_cnt;
 	int armors_cnt;
 	int count;
diff --git a/energy/include/energy/param_struct_define.h b/energy/include/energy/param_struct_define.h
index 5c8209f..f222368 100644
--- a/energy/include/energy/param_struct_define.h
+++ b/energy/include/energy/param_struct_define.h
@@ -49,6 +49,16 @@ struct EnergyPartParam {
 	float TWIN_ANGEL_MAX;
 };
 
+struct LiftHeight{
+    float LIFT_0;
+    float LIFT_30;
+    float LIFT_60;
+    float LIFT_90;
+    float LIFT_minus_30;
+    float LIFT_minus_60;
+    float LIFT_minus_90;
+};
+
 typedef struct GMAngle_t{
     float yaw;
     float pitch;
diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp
index 4af8838..22f68f2 100644
--- a/energy/src/energy/get/gimble_rotation_get.cpp
+++ b/energy/src/energy/get/gimble_rotation_get.cpp
@@ -23,8 +23,8 @@ void Energy::gimbleRotation(){
 //    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(YAW_ORIGIN_RAD)-real_hit_point.x), ATTACK_DISTANCE));
 //    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-real_hit_point.y), ATTACK_DISTANCE));
 
-    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(mark_yaw*PI/180)-real_hit_point.x), ATTACK_DISTANCE));
-    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(mark_pitch*PI/180)-real_hit_point.y), ATTACK_DISTANCE));
+    yaw_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_yaw*PI/180)-real_hit_point.x), attack_distance));
+    pitch_rotation = static_cast<float>(180 / PI * atan2((attack_distance*tan(mark_pitch*PI/180)-real_hit_point.y), attack_distance));
 
 //    yaw_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(3.5*PI/180)-STRETCH*(hit_point.x-cycle_center.x)), ATTACK_DISTANCE));
 //    pitch_rotation = static_cast<float>(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/param_init.cpp b/energy/src/energy/param_init.cpp
index a81ca0e..36bcc3c 100644
--- a/energy/src/energy/param_init.cpp
+++ b/energy/src/energy/param_init.cpp
@@ -11,7 +11,7 @@ using std::vector;
 void Energy::initEnergyPartParam() {
 
     energy_part_param_.RPM = 10;
-    energy_part_param_.HIT_TIME = 1.0;
+    energy_part_param_.HIT_TIME = 1.5;
 
 	energy_part_param_.GRAY_THRESH = 240;
 	energy_part_param_.SPLIT_GRAY_THRESH = 80;
@@ -36,6 +36,13 @@ void Energy::initEnergyPartParam() {
 
 	energy_part_param_.TWIN_ANGEL_MAX = 10;
 
+	lift_height_.LIFT_0 = 0;
+    lift_height_.LIFT_30 = 0;
+    lift_height_.LIFT_60 = 0;
+    lift_height_.LIFT_90 = 10;
+    lift_height_.LIFT_minus_30 = 0;
+    lift_height_.LIFT_minus_60 = 0;
+    lift_height_.LIFT_minus_90 = 0;
 
 }
 
diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp
index dfdbae9..99e2217 100644
--- a/energy/src/energy/run.cpp
+++ b/energy/src/energy/run.cpp
@@ -12,6 +12,7 @@ extern float curr_yaw, curr_pitch, mark_yaw, mark_pitch;
 extern int mark;
 
 int Energy::run(cv::Mat &src){
+//    imshow("src",src);
     fans.clear();
     armors.clear();
     fanPosition.clear();
@@ -31,11 +32,11 @@ int Energy::run(cv::Mat &src){
 //    imshow("bin",src);
 
 
-//    fans_cnt = findFan(src, fans, last_fans_cnt);
+    fans_cnt = findFan(src, fans, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
-//    if(fans_cnt==-1) return 0;//滤去漏判的帧
+    if(fans_cnt==-1) return 0;//滤去漏判的帧
 //    if(fans_cnt>0)showFanContours("fan",src,fans);
-    fans_cnt=0;
+//    fans_cnt=0;
 
     armors_cnt = findArmor(src, armors, last_armors_cnt);
 //    cout<<"armors_cnt: "<<armors_cnt<<endl;
@@ -61,8 +62,11 @@ int Energy::run(cv::Mat &src){
     cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
     cycleLeastFit();
 
-    cycle_center = cv::Point(414,227);
-    radius = 203.728;
+
+    cycle_center = cv::Point(332,233);
+    radius = 208.835;
+    attack_distance = ATTACK_DISTANCE * 203.728 / radius;
+
     getFanPosition(fanPosition, fans, cycle_center, radius);
     getArmorPosition(armorPosition, armors, cycle_center, radius);
     findTarget(fanPosition, armorPosition, target_armor);