From 6e69bb0bd2a3b81c053431a11e02c95ec8c4bd6e Mon Sep 17 00:00:00 2001
From: sun <SJT_sjtu1998@163.con>
Date: Sun, 4 Aug 2019 23:36:51 +0800
Subject: [PATCH] energy changed

---
 energy/src/energy/send/send.cpp   | 14 +++++++++++++-
 others/include/config/setconfig.h | 16 ++++++++--------
 2 files changed, 21 insertions(+), 9 deletions(-)

diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp
index c140ae8..6ef0d10 100644
--- a/energy/src/energy/send/send.cpp
+++ b/energy/src/energy/send/send.cpp
@@ -19,23 +19,35 @@ void Energy::sendEnergy() {
             sum_yaw += yaw_rotation;
             sum_pitch += pitch_rotation;
             MINMAX(sum_yaw, -100, 100);
-            MINMAX(sum_pitch, -100, 100);\
+            MINMAX(sum_pitch, -100, 100);
+            double tmp_yaw = yaw_rotation;
+            double tmp_pitch = pitch_rotation;
             yaw_rotation = BIG_YAW_AIM_KP * yaw_rotation + BIG_YAW_AIM_KI * sum_yaw + BIG_YAW_AIM_KD * (yaw_rotation - last_yaw);
             pitch_rotation = BIG_PITCH_AIM_KP * pitch_rotation + BIG_PITCH_AIM_KI * sum_pitch +
                     BIG_PITCH_AIM_KD * (pitch_rotation - last_pitch);
+            last_yaw = tmp_yaw;
+            last_pitch = tmp_pitch;
         } else if (is_chassis) {
             sum_yaw += yaw_rotation - mcuData.curr_yaw;
             sum_pitch += pitch_rotation - mcuData.curr_pitch;
+            double tmp_yaw = yaw_rotation;
+            double tmp_pitch = pitch_rotation;
             yaw_rotation = BIG_YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + BIG_YAW_AIM_KI * sum_yaw;
             pitch_rotation = BIG_PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + BIG_PITCH_AIM_KI * sum_pitch;
+            last_yaw = tmp_yaw;
+            last_pitch = tmp_pitch;
         }
     } else if (is_small){
         sum_yaw += yaw_rotation;
         sum_pitch += pitch_rotation;
         MINMAX(sum_yaw, -100, 100);
         MINMAX(sum_pitch, -100, 100);
+        double tmp_yaw = yaw_rotation;
+        double tmp_pitch = pitch_rotation;
         yaw_rotation = SMALL_YAW_AIM_KP * yaw_rotation + SMALL_YAW_AIM_KD * (yaw_rotation - last_yaw);
         pitch_rotation = SMALL_PITCH_AIM_KP * pitch_rotation + SMALL_PITCH_AIM_KD * (pitch_rotation - last_pitch);
+        last_yaw = tmp_yaw;
+        last_pitch = tmp_pitch;
     }
 
 
diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h
index 7a7c34e..0768d1c 100644
--- a/others/include/config/setconfig.h
+++ b/others/include/config/setconfig.h
@@ -32,31 +32,31 @@
     #define ENERGY_CAMERA_GAIN  (20)
 #endif
 #ifndef SMALL_YAW_AIM_KD
-    #define SMALL_YAW_AIM_KD (1.5)
+    #define SMALL_YAW_AIM_KD (0)
 #endif
 #ifndef SMALL_YAW_AIM_KP
-    #define SMALL_YAW_AIM_KP (2.5)
+    #define SMALL_YAW_AIM_KP (4)
 #endif
 #ifndef SMALL_PITCH_AIM_KD
-    #define SMALL_PITCH_AIM_KD (1.3)
+    #define SMALL_PITCH_AIM_KD (0)
 #endif
 #ifndef SMALL_PITCH_AIM_KP
-    #define SMALL_PITCH_AIM_KP (2.4)
+    #define SMALL_PITCH_AIM_KP (3.7)
 #endif
 #ifndef BIG_YAW_AIM_KD
-    #define BIG_YAW_AIM_KD (1)
+    #define BIG_YAW_AIM_KD (0)
 #endif
 #ifndef BIG_YAW_AIM_KP
-    #define BIG_YAW_AIM_KP (5.5)
+    #define BIG_YAW_AIM_KP (6.5)
 #endif
 #ifndef BIG_YAW_AIM_KI
     #define BIG_YAW_AIM_KI (0.1)
 #endif
 #ifndef BIG_PITCH_AIM_KD
-    #define BIG_PITCH_AIM_KD (1)
+    #define BIG_PITCH_AIM_KD (0)
 #endif
 #ifndef BIG_PITCH_AIM_KP
-    #define BIG_PITCH_AIM_KP (5.5)
+    #define BIG_PITCH_AIM_KP (6.5)
 #endif
 #ifndef BIG_PITCH_AIM_KI
     #define BIG_PITCH_AIM_KI (0.1)