//
// 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;
}