move to ccs based on 39cc590260f6d69837dd16740b7b8612e8935592

3rd/pid_control.c

@@ -0,0 +1,51 @@
+#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)));
+}
+
+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;
+    }
+
+    if(Ts <= 0 || Ts > 0.05f)
+    {
+        Ts = 0.001;
+    }
+
+    float proportion = Kp * Error;//P环
+
+    float intergration = Last_intergration + Ki * 0.5f * Ts * Error;//I环
+    intergration = _constrain(intergration, -limit, limit);
+
+    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;
+}
+