pref: 分段pid更新

app/jj_motion.c

@@ -12,46 +12,47 @@ PID_TypeDef far_gyro_pid;
 PID_TypeDef near_pos_pid;
 PID_TypeDef speed_pid;
 
-float an_Kp0 = 8.0f;
-float an_Ki0 = 1.0f;
-float an_Kd0 = 2.0f;
+float an_Kp0 = 80.0f;
+float an_Ki0 = 1.4f;
+float an_Kd0 = 8.0f;
 
-float an_Kp1 = 8.0f;
-float an_Ki1 = 1.0f;
-float an_Kd1 = 2.0f;
+float an_Kp1 = 80.0f;
+float an_Ki1 = 1.4f;
+float an_Kd1 = 8.0f;
 float in_angle;
 float set_angle = 0.0f;
 float out_angle;
 
-float gy_Kp0 = 1.0f;
+float gy_Kp0 = 2.0f;
 float gy_Ki0 = 0.0f;
-float gy_Kd0 = 0.0f;
+float gy_Kd0 = 0.3f;
 
-float gy_Kp1 = 1.0f;
+float gy_Kp1 = 2.0f;
 float gy_Ki1 = 0.0f;
-float gy_Kd1 = 0.0f;
+float gy_Kd1 = 0.3f;
 float in_gyro;
 float out_gyro;
 // float set_gyro = 0.0f;
 
-float po_Kp0 = 1.0f;
-float po_Ki0 = 0.0f;
-float po_Kd0 = 0.0f;
+float po_Kp0 = 827.0f;
+float po_Ki0 = 16.0f;
+float po_Kd0 = 13.0f;
 
-float po_Kp1 = 1.0f;
-float po_Ki1 = 0.0f;
-float po_Kd1 = 0.0f;
+float po_Kp1 = 500.0f;
+float po_Ki1 = 2.0f;
+float po_Kd1 = 1.0f;
 float in_pos;
 float out_pos;
 float set_pos = 0.0f;
 
-float sp_Kp = 1.0f;
-float sp_Ki = 0.0f;
+float sp_Kp = 19.0f;
+float sp_Ki = 0.5f;
 float sp_Kd = 0.0f;
 float in_speed;
 float out_speed;
-float set_speed0  = 0.0f;
-float set_speed1  = 0.0f;
+
+float set_speed0  = 460.0f;
+float set_speed1  = 460.0f;
 int cnt1          = 0;
 int cnt2          = 0;
 uint32_t in_state = 0;
@@ -87,8 +88,12 @@ void sport_motion(void)
 {
     if (1 == in_state) {
         PID_SetTunings(&far_gyro_pid, an_Kp1, an_Ki1, an_Kd1);
+        PID_SetTunings(&near_pos_pid, po_Kp1, po_Ki1, po_Kd1);
+        PID_SetTunings(&far_gyro_pid, gy_Kp1, gy_Ki1, gy_Kd1);
     } else {
         PID_SetTunings(&far_gyro_pid, an_Kp0, an_Ki0, an_Kd0);
+        PID_SetTunings(&near_pos_pid, po_Kp0, po_Ki0, po_Kd0);
+        PID_SetTunings(&far_gyro_pid, gy_Kp0, gy_Ki0, gy_Kd0);
     }
     /* 动力风扇设置 */
     if (1 == bt_run_flag) {