51 lines
1.2 KiB
C
51 lines
1.2 KiB
C
#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);
|
|
|
|
}
|