chenged to 2 motor velocity-closed-loop; before testing

3rd/pid_control.c

@@ -1,51 +1,41 @@
+#include "pid_control.h"
 #include <stdint.h>
 #include <ti_msp_dl_config.h>
-#define  limit            5.0
-#define  Output_ramp      10000
 
-//限幅
-float  _constrain(float amt, float low, float high)
-{
-    return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
+#define limit 5.0
+#define Output_ramp 10000
+
+// 限幅
+float _constrain(float amt, float low, float high) {
+  return ((amt < low) ? (low) : ((amt) > (high) ? (high) : (amt)));
 }
 
-unsigned long Timestamp_Last = 0.0;
-float Last_Error = 0.0;
-float Last_intergration = 0.0;
-float Last_Output = 0.0;
-float PID_Controller(float Kp, float Ki, float Kd, float Error)
-{
-    float Ts = 0.0;
-    uint32_t Timestamp = DL_SYSTICK_getValue();
-    if(Timestamp < Timestamp_Last)
-    {
-        Ts = (float)(Timestamp_Last - Timestamp) / 80 * 1e-6;
-    }
-    else
-    {
-        Ts = (0xFFFFFF - Timestamp + Timestamp_Last) / 80 * 1e-6;
-    }
+float PID_Controller(struct _PID *pid, float error) {
+  float Ts = 0.0;
+  uint32_t Timestamp = DL_SYSTICK_getValue(); // 假设这里是正确获取时间戳的方式
+  if (Timestamp < pid->Timestamp_Last)
+    Ts = (float)(pid->Timestamp_Last - Timestamp) / 9 * 1e-6;
+  else
+    Ts = (0xFFFFFF - Timestamp + pid->Timestamp_Last) / 9 * 1e-6;
 
-    if(Ts <= 0 || Ts > 0.05f)
-    {
-        Ts = 0.001;
-    }
+  if (Ts <= 0 || Ts > 0.05f)
+    Ts = 0.001;
 
-    float proportion = Kp * Error;//P环
+  float proportion = pid->Kp * error; // P环
 
-    float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
-    intergration = _constrain(intergration, -limit, limit);
+  float intergration =
+      pid->Last_intergration + pid->Ki * 0.5f * Ts * error; // I环
+  // 假设 _constrain 函数可以对 intergration 进行限制
+  intergration = _constrain(intergration, -limit, limit);
+  float differential = pid->Kd * (error - pid->Last_Error) / Ts; // D环
 
-    float differential = Kd * (Error - Last_Error) / Ts; //D环
-
-    float Output = proportion + intergration + differential;
-    Output = _constrain(Output, -limit, limit);
-
-    Last_Error = Error;
-    Last_intergration = intergration;
-    Last_Output = Output;
-    Timestamp_Last = Timestamp;
-
-    return Output;
-}
+  float Output = proportion + intergration + differential;
+  // 假设 _constrain 函数可以对 Output 进行限制
+  Output = _constrain(Output, -limit, limit);
+  pid->Last_Error = error;
+  pid->Last_intergration = intergration;
+  pid->Last_Output = Output;
+  pid->Timestamp_Last = Timestamp;
 
+  return Output;
+}