27 lines
702 B
C
27 lines
702 B
C
//
|
|
// 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;
|
|
} |