原始版本

stm32f407vet6_v2/applications/pid.c

@@ -0,0 +1,50 @@
+#include <sys/time.h>
+#include <rtthread.h>
+
+#include "pid.h"
+
+void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd)
+{
+    pid->kp = kp;
+    pid->ki = ki;
+    pid->kd = kd;
+    pid->setpoint = 0;
+    pid->integral = 0;
+    pid->prev_error = 0;
+    pid->output = 0;
+    pid->last_time = 0;
+}
+
+float PID_Calculate(PID_TypeDef *pid, float measured, unsigned long now_time)
+{
+    float error = pid->setpoint - measured;
+    float delta_time = (now_time - pid->last_time) / 1000.0f; // 转换为秒
+
+    if (delta_time <= 0)
+    {
+        return pid->output; // 如果时间间隔无效，返回上次输出
+    }
+
+    pid->integral += error * delta_time;
+    float derivative = (error - pid->prev_error) / delta_time;
+
+    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;
+
+    pid->prev_error = error;
+    pid->last_time = now_time;
+
+    return pid->output;
+}
+
+extern unsigned long get_timestamp(void);
+
+void pid_demo(void)
+{
+    PID_TypeDef pid;
+    PID_Init(&pid, 1.0f, 0.5f, 0.1f);
+    pid.setpoint = 100.0f; // 设置目标值
+    // unsigned long now_time = get_timestamp(); // 获取当前时间戳
+
+    // float output = PID_Calcuate(&pid, measured, now);
+    
+}