From 6615bda7a922f57e1b9048f2c37be8bae1dc3a6c Mon Sep 17 00:00:00 2001
From: sun <SJT_sjtu1998@163.con>
Date: Fri, 12 Jul 2019 21:26:01 +0800
Subject: [PATCH] energy changed

---
 energy/include/energy/energy.h         |  3 ++-
 energy/src/energy/get/origin_get.cpp   | 25 +++++++++++++++++--------
 energy/src/energy/init/energy_init.cpp |  2 ++
 energy/src/energy/run.cpp              |  6 +++---
 4 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h
index 9aee3a1..173c901 100644
--- a/energy/include/energy/energy.h
+++ b/energy/include/energy/energy.h
@@ -76,6 +76,7 @@ private:
     float last_base_angle;//上一帧的各扇叶在0区（0°~72°）的基础角度
     float predict_rad;//预测提前角
     float attack_distance;//步兵与风车平面距离
+    float center_delta_yaw, center_delta_pitch;//对心时相差的角度
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float origin_yaw, origin_pitch;//初始的云台角度设定值
 
@@ -148,7 +149,7 @@ private:
 
     bool guessTarget();//获得猜测击打点位
     bool changeTarget();//判断目标是否改变
-    void getOrigin();//获得云台对心所需角度
+    bool getOrigin();//获得云台对心所需角度
     void getTargetPolarAngle();//获得目标装甲板极坐标角度
     void getPredictPoint(cv::Point target_point);//获取预测点位
     void getAimPoint(cv::Point target_point);//通过自瞄逻辑计算点位
diff --git a/energy/src/energy/get/origin_get.cpp b/energy/src/energy/get/origin_get.cpp
index 2dbd061..83ddcff 100644
--- a/energy/src/energy/get/origin_get.cpp
+++ b/energy/src/energy/get/origin_get.cpp
@@ -11,14 +11,23 @@ using namespace cv;
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于获得云台对心得到的初始yaw和pitch（即以该yaw和pitch发射子弹，可以击中风车中心）
 // ---------------------------------------------------------------------------------------------------------------------
-void Energy::getOrigin() {
+bool Energy::getOrigin() {
     if (!auto_mark && !manual_mark) {
-        double dx = circle_center_point.x - 320;
-        double dy = circle_center_point.y - 240;
-        origin_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
-        origin_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
-
-        auto_mark = true;
-        LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
+        double dx = -(circle_center_point.x - 320);
+        double dy = -(circle_center_point.y - 240);
+        center_delta_yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+        center_delta_pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
+        if (abs(center_delta_pitch) > 0.3 || abs(center_delta_pitch) > 0.3) {
+            sendTarget(serial, center_delta_yaw, center_delta_pitch, false);
+            return false;
+        } else {
+            origin_yaw = mcuData.curr_yaw;
+            origin_pitch = mcuData.curr_pitch;
+            auto_mark = true;
+            sendTarget(serial, center_delta_yaw, center_delta_pitch, true);
+            LOGM(STR_CTR(WORD_BLUE_CODE, "auto mark success!"));
+            return true;
+        }
     }
+    return true;
 }
\ No newline at end of file
diff --git a/energy/src/energy/init/energy_init.cpp b/energy/src/energy/init/energy_init.cpp
index 67ae5da..2d0c040 100644
--- a/energy/src/energy/init/energy_init.cpp
+++ b/energy/src/energy/init/energy_init.cpp
@@ -51,6 +51,8 @@ void Energy::initEnergy() {
 
     clockwise_rotation_init_cnt = 0;
     anticlockwise_rotation_init_cnt = 0;
+
+    destroyAllWindows();
 }
 
 
diff --git a/energy/src/energy/run.cpp b/energy/src/energy/run.cpp
index 4c41352..b984efe 100644
--- a/energy/src/energy/run.cpp
+++ b/energy/src/energy/run.cpp
@@ -29,10 +29,10 @@ void Energy::run(cv::Mat &gimble_src, cv::Mat &chassis_src) {
         if (show_energy)showFlowStripFan("strip", gimble_src);
         if (show_energy)showCenterR("R", gimble_src);
 
+        if (!getOrigin())return;
         startChassis();
-        getOrigin();
         initEnergy();
-        destroyAllWindows();
+
     } else if (isChassis) {
         energy_part_param_ = chassis_energy_part_param_;
         clearAll();
@@ -97,7 +97,7 @@ void Energy::run(cv::Mat &gimble_src) {
     getTargetPolarAngle();
     JudgeMode();
     if (energy_mode_init)return;
-    getOrigin();
+    if (!getOrigin())return;
     if (isBig & energy_rotation_init) {
         initRotation();
         return;