diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h
index a9e25f0..3a6da62 100644
--- a/energy/include/energy/energy.h
+++ b/energy/include/energy/energy.h
@@ -85,6 +85,7 @@ private:
     float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度
     float origin_yaw, origin_pitch;//初始的云台角度设定值
     float shoot;//若为2，则要求主控板发弹
+    float last_yaw, last_pitch;//PID中微分项
     float sum_yaw, sum_pitch;//yaw和pitch的累计误差，即PID中积分项
 
     systime time_start_guess;
diff --git a/energy/src/energy/clear/energy_init.cpp b/energy/src/energy/clear/energy_init.cpp
index 1d11c6c..5a89d84 100644
--- a/energy/src/energy/clear/energy_init.cpp
+++ b/energy/src/energy/clear/energy_init.cpp
@@ -52,6 +52,8 @@ void Energy::initEnergy() {
     yaw_rotation = 0;
     pitch_rotation = 0;
     shoot = 0;
+    last_yaw = 0;
+    last_pitch = 0;
     sum_yaw = 0;
     sum_pitch = 0;
 
diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp
index 281dcf1..deece8a 100644
--- a/energy/src/energy/send/send.cpp
+++ b/energy/src/energy/send/send.cpp
@@ -8,6 +8,7 @@
 
 using namespace std;
 
+#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
 //----------------------------------------------------------------------------------------------------------------------
 // 此函数用于发送能量机关数据
@@ -17,13 +18,20 @@ void Energy::sendEnergy() {
         if (camera_cnt == 1) {
             sum_yaw += yaw_rotation;
             sum_pitch += pitch_rotation;
-            yaw_rotation = AIM_KP * yaw_rotation + AIM_KI * sum_yaw;
-            pitch_rotation = AIM_KP * pitch_rotation + AIM_KI * sum_pitch;
+            MINMAX(sum_yaw, -100, 100);
+            MINMAX(sum_pitch, -100, 100);
+//            float KP = 4.5;
+//            if(abs(yaw_rotation)<0.3)KP=0.1;
+//            else if(abs(yaw_rotation)<0.5)KP=0.1;
+//            else KP = 0.1; " << yaw_rotation << '\t' << "pitch: " << pitch_rotation << endl;
+            yaw_rotation = YAW_AIM_KP * yaw_rotation + YAW_AIM_KI * sum_yaw + YAW_AIM_KD * (yaw_rotation - last_yaw);
+            pitch_rotation = PITCH_AIM_KP * pitch_rotation + PITCH_AIM_KI * sum_pitch +
+                             PITCH_AIM_KD * (pitch_rotation - last_pitch);
         } else if (is_chassis) {
             sum_yaw += yaw_rotation - mcuData.curr_yaw;
             sum_pitch += pitch_rotation - mcuData.curr_pitch;
-            yaw_rotation = AIM_KP * (yaw_rotation - mcuData.curr_yaw) + AIM_KI * sum_yaw;
-            pitch_rotation = AIM_KP * (pitch_rotation - mcuData.curr_pitch) + AIM_KI * sum_pitch;
+            yaw_rotation = YAW_AIM_KP * (yaw_rotation - mcuData.curr_yaw) + YAW_AIM_KI * sum_yaw;
+            pitch_rotation = PITCH_AIM_KP * (pitch_rotation - mcuData.curr_pitch) + PITCH_AIM_KI * sum_pitch;
         }
     }
 
@@ -51,7 +59,7 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z) {
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z  << ")" << endl;
+        cout << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
         fps = 0;
     }
     fps += 1;
diff --git a/others/include/config/setconfig.h b/others/include/config/setconfig.h
index e23db56..cf35c9e 100644
--- a/others/include/config/setconfig.h
+++ b/others/include/config/setconfig.h
@@ -31,11 +31,23 @@
 #ifndef ENERGY_CAMERA_GAIN
     #define ENERGY_CAMERA_GAIN  (30)
 #endif
-#ifndef AIM_KP
-    #define AIM_KP (6)
+#ifndef YAW_AIM_KD
+    #define YAW_AIM_KD (0)
 #endif
-#ifndef AIM_KI
-    #define AIM_KI (0.1)
+#ifndef YAW_AIM_KP
+    #define YAW_AIM_KP (6)
+#endif
+#ifndef YAW_AIM_KI
+    #define YAW_AIM_KI (0.1)
+#endif
+#ifndef PITCH_AIM_KD
+    #define PITCH_AIM_KD (0)
+#endif
+#ifndef PITCH_AIM_KP
+    #define PITCH_AIM_KP (6)
+#endif
+#ifndef PITCH_AIM_KI
+    #define PITCH_AIM_KI (0.1)
 #endif
 #ifndef COMPENSATE_YAW
     #define COMPENSATE_YAW (5)