变更

BoooomCTL/Common/PID/PID.c

@@ -0,0 +1,27 @@
+//
+// Created by ZK on 2023/3/14.
+//
+
+#include "PID.h"
+
+float PID_Generate(PID *pid) {
+    pid->error = pid->target - pid->value;
+
+    if (pid->error > pid->errMin || pid->error < -pid->errMin)
+        pid->errSum += pid->error * pid->ki;
+
+    if (pid->errSum > pid->errSumMax)
+        pid->errSum = pid->errSumMax;
+    else if (pid->errSum < -pid->errSumMax)
+        pid->errSum = -pid->errSumMax;
+
+    pid->errDt = pid->error - pid->lastErr;
+    pid->lastErr = pid->error;
+
+    pid->result = pid->kp * pid->error + pid->errSum + pid->kd * pid->errDt;
+
+    if (pid->result > pid->valMax)
+        pid->result = pid->valMax;
+    else if (pid->result < -pid->valMax)
+        pid->result = -pid->valMax;
+}

BoooomCTL/Common/PID/PID.h

@@ -0,0 +1,32 @@
+//
+// Created by ZK on 2023/3/14.
+//
+
+#ifndef BOOOOMFOC_STSPIN32G4_EVB_PID_H
+#define BOOOOMFOC_STSPIN32G4_EVB_PID_H
+
+typedef struct pid {
+    float kp;//比例系数
+    float ki;//积分系数
+    float kd;//微分系数
+
+    float target;
+    float value;
+    float error;
+
+    float errSum;
+    float errSumMax;
+
+    float errMin;
+    float valMax;
+
+    float lastErr;
+    float errDt;
+
+    float result;
+} PID;
+
+float PID_Generate(PID *pid);
+
+
+#endif //BOOOOMFOC_STSPIN32G4_EVB_PID_H