From 6f517f7853f7b521b86f2205ff92a6a809e56136 Mon Sep 17 00:00:00 2001
From: sun <SJT_sjtu1998@163.con>
Date: Fri, 19 Jul 2019 23:48:33 +0800
Subject: [PATCH] energy change

---
 energy/src/energy/get/aim_point_get.cpp | 10 ++++++++--
 energy/src/energy/judge/judge_shoot.cpp |  7 +++++--
 others/include/config/setconfig.h       | 12 ++++++++++++
 3 files changed, 25 insertions(+), 4 deletions(-)

diff --git a/energy/src/energy/get/aim_point_get.cpp b/energy/src/energy/get/aim_point_get.cpp
index 189811b..4c3ac74 100644
--- a/energy/src/energy/get/aim_point_get.cpp
+++ b/energy/src/energy/get/aim_point_get.cpp
@@ -4,6 +4,7 @@
 
 #include "energy/energy.h"
 #include "energy/constant.h"
+#include "config/setconfig.h"
 
 using namespace std;
 using namespace cv;
@@ -12,12 +13,17 @@ using namespace cv;
 // 此函数通过自瞄逻辑击打目标点，用于大符的自动对心和小符直接打击
 // ---------------------------------------------------------------------------------------------------------------------
 void Energy::getAimPoint(cv::Point target_point) {
+    double dx, dy;
 //五号车
 //    double dx = -(target_point.x - 320 - 10);
 //    double dy = -(target_point.y - 240 - 22);
 //四号车
-    double dx = -(target_point.x - 320 - 5);
-    double dy = -(target_point.y - 240 - 74);
+#ifndef COMPENSATE_YAW
+    dx = -(target_point.x - 320 - COMPENSATE_YAW);
+#endif
+#ifndef COMPENSATE_PITCH
+    dy = -(target_point.y - 240 - COMPENSATE_PITCH);
+#endif
     yaw_rotation = atan(dx / FOCUS_PIXAL) * 180 / PI;
     pitch_rotation = atan(dy / FOCUS_PIXAL) * 180 / PI;
 //    cout << "yaw: " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
diff --git a/energy/src/energy/judge/judge_shoot.cpp b/energy/src/energy/judge/judge_shoot.cpp
index 1407479..7204316 100644
--- a/energy/src/energy/judge/judge_shoot.cpp
+++ b/energy/src/energy/judge/judge_shoot.cpp
@@ -4,6 +4,7 @@
 
 #include "energy/energy.h"
 #include "log.h"
+#include "config/setconfig.h"
 
 using namespace std;
 using namespace cv;
@@ -43,6 +44,8 @@ void Energy::judgeShootInGimbal(){
     static float sum_pitch = 0;
     sum_yaw += yaw_rotation;
     sum_pitch += pitch_rotation;
-    yaw_rotation = 6 * yaw_rotation + 0.1 * sum_yaw;
-    pitch_rotation = 6 * pitch_rotation + 0.1 * sum_pitch;
+#ifndef AIM_KP  AIM_KI
+    yaw_rotation = AIM_KP * yaw_rotation +  AIM_KI * sum_yaw;
+    pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
+#endif
 }
diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h
index 86412d7..5541be1 100644
--- a/others/include/config/setconfig.h
+++ b/others/include/config/setconfig.h
@@ -30,6 +30,18 @@
 #ifndef ENERGY_CAMERA_GAIN
     #define ENERGY_CAMERA_GAIN  (20)
 #endif
+#ifndef AIM_KP
+    #define AIM_KP (6)
+#endif
+#ifndef AIM_KI
+    #define AIM_KI (0.1)
+#endif
+#ifndef COMPENSATE_YAW
+    #define COMPENSATE_YAW (5)
+#endif
+#ifndef COMPENSATE_PITCH
+    #define COMPENSATE_PITCH (74)
+#endif
 
 //#define GIMBAL_FLIP_MODE    (-1)
 //#define CHASSIS_FLIP_MODE   (-1)