From 2e03e806531016aa6443d84ffd6085ea95aaac68 Mon Sep 17 00:00:00 2001
From: JiatongSun <SJT_sjtu1998@163.com>
Date: Mon, 15 Apr 2019 17:15:16 +0800
Subject: [PATCH] Auto Aim

---
 energy/src/energy/get/gimble_rotation_get.cpp | 16 ++++++------
 energy/src/energy/run.cpp                     | 25 +++++++++++++------
 energy/src/energy/show/show.cpp               |  4 +++
 main.cpp                                      |  4 ++-
 4 files changed, 33 insertions(+), 16 deletions(-)

diff --git a/energy/src/energy/get/gimble_rotation_get.cpp b/energy/src/energy/get/gimble_rotation_get.cpp
index 664670a..4af8838 100644
--- a/energy/src/energy/get/gimble_rotation_get.cpp
+++ b/energy/src/energy/get/gimble_rotation_get.cpp
@@ -18,16 +18,16 @@ void Energy::gimbleRotation(){
 //    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(PITCH_ORIGIN_RAD)-STRETCH*(hit_point.y-ZERO_POINT_Y)), ATTACK_DISTANCE));
 
 //该方法用于操作手自己完成对心工作的情况，对操作手要求高
-//    cv::Point2f real_hit_point;
-//    stretch(hit_point, real_hit_point);
+    cv::Point2f real_hit_point;
+    stretch(hit_point, real_hit_point);
 //    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(curr_yaw)-real_hit_point.x), ATTACK_DISTANCE));
-//    pitch_rotation = static_cast<float>(180 / PI * atan2((ATTACK_DISTANCE*tan(curr_pitch)-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));
-    cout<<"cur: "<<curr_yaw<<endl;
-    cout<<"hit point: "<<hit_point<<'\t'<<"cycle center: "<<cycle_center<<endl;
+//    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));
+//    cout<<"cur: "<<curr_yaw<<endl;
+//    cout<<"hit point: "<<hit_point<<'\t'<<"cycle center: "<<cycle_center<<endl;
 }
\ No newline at end of file
diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp
index 8b46f92..20e3169 100644
--- a/energy/src/energy/run.cpp
+++ b/energy/src/energy/run.cpp
@@ -8,12 +8,18 @@ using std::cout;
 using std::endl;
 using std::vector;
 
+extern float mark_yaw, mark_pitch;
+extern int mark;
+
 int Energy::run(cv::Mat &src){
     fans.clear();
     armors.clear();
     fanPosition.clear();
     armorPosition.clear();
     gimble_zero_points.clear();
+
+    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(): "<<first_armor_centers.size()<<endl;
@@ -24,7 +30,7 @@ int Energy::run(cv::Mat &src){
     threshold(src, src, energy_part_param_.GRAY_THRESH, 255, THRESH_BINARY);
 //    imshow("bin",src);
 
-/*
+
     fans_cnt = findFan(src, fans, last_fans_cnt);
 //    cout<<"fans_cnt: "<<fans_cnt<<endl;
     if(fans_cnt==-1) return 0;//滤去漏判的帧
@@ -39,22 +45,24 @@ int Energy::run(cv::Mat &src){
     if(armors_cnt>0||fans_cnt>0) showBothContours("Both",src, fans, armors);
 
     if(armors_cnt != fans_cnt+1) return 0;
-*/
 
+
+/*
 //此处用于标定云台在摄像头视频中的零点
     findGimbleZeroPoint(src,gimble_zero_points);
     cout<<"gimble zero points: :"<<gimble_zero_points.size()<<endl;
     showFanContours("zero",src,gimble_zero_points);
     cycle_center = cv::Point(291,305);
     if(gimble_zero_points.size()>0)hit_point = gimble_zero_points.at(0).rect.center;
+*/
+
 
-/*
     getAllArmorCenters();
     cout<<"all_armor_centers.size(): "<<all_armor_centers.size()<<endl;
     cycleLeastFit();
 
-//    cycle_center = cv::Point(248,247);
-//    radius = 208.439;
+    cycle_center = cv::Point(416,227);
+    radius = 203.665;
     getFanPosition(fanPosition, fans, cycle_center, radius);
     getArmorPosition(armorPosition, armors, cycle_center, radius);
     findTarget(fanPosition, armorPosition, target_armor);
@@ -62,14 +70,17 @@ int Energy::run(cv::Mat &src){
     cout<<"The target armor center is: "<<target_center<<endl;
 
     getHitPoint();
+    hit_point = target_center;
     cout << "The hit point position is " << hit_point << endl;
 //    hit_point = cycle_center;
-*/
+
     gimbleRotation();
 
     sendTargetByUart(yaw_rotation, pitch_rotation, attack_distance);
     cout<<"yaw: "<<yaw_rotation<<'\t'<<"pitch: "<<pitch_rotation<<endl;
-//    cout<<"send_cnt: "<<send_cnt<<endl;
+    cout<<"mark_yaw: "<<mark_yaw<<'\t'<<"mark_pitch: "<<mark_pitch<<endl;
+
+    cout<<"send_cnt: "<<send_cnt<<endl;
 
 
 
diff --git a/energy/src/energy/show/show.cpp b/energy/src/energy/show/show.cpp
index 92a8aad..d315df6 100644
--- a/energy/src/energy/show/show.cpp
+++ b/energy/src/energy/show/show.cpp
@@ -88,6 +88,10 @@ void Energy::showBothContours(std::string windows_name, const cv::Mat &src, cons
 			line(image2show, vertices[i], vertices[(i + 1) % 4], Scalar(0, 0, 255), 4);
 //		cout << "armor center: "<< armor.rect.center << '\t'<< "armor angle: "<< armor.rect.angle;
 //		cout << endl;
+
+        cv::Point2f point = armor.rect.center;
+        cv::circle(image2show, point, 2, cv::Scalar(0, 0, 255));//在图像中画出特征点，2是圆的半径
+
 	}
 
     imshow(windows_name, image2show);
diff --git a/main.cpp b/main.cpp
index 795b4c4..b7ef410 100644
--- a/main.cpp
+++ b/main.cpp
@@ -27,6 +27,7 @@ using namespace std;
 int state = ENERGY_STATE;
 float curr_yaw=0, curr_pitch=0;
 float mark_yaw=0, mark_pitch=0;
+int mark = 0;
 
 void uartReceive(Uart* uart);
 
@@ -112,8 +113,9 @@ void uartReceive(Uart* uart){
             }
             memcpy(&curr_yaw, buffer, 4);
             memcpy(&curr_pitch, buffer+4, 4);
-            LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
+//            LOGM("Get yaw:%f pitch:%f", curr_yaw, curr_pitch);
             if(buffer[9] == 1){
+                mark = 1;
                 mark_yaw = curr_yaw;
                 mark_pitch = curr_pitch;
 //                LOGM("Marked");