增量型PID算法

#include <iostream>using namespace std;struct _PID{    float SetSpeed;    float ActualSpeed;    float Kp;    float Ki;    float Kd;    float err;    float Last_err;    float Up_err;    float PID_increase;}PID;void PID_init (){    PID.SetSpeed = 0;    PID.ActualSpeed = 0;    PID.Kp = 0.2;    PID.Ki = 0.015;    PID.Kd = 0.2;    PID.err = 0;    PID.Last_err = 0;    PID.Up_err  = 0;    PID.PID_increase = 0;}float PID_Algorithm (float speed){    PID.SetSpeed = speed;    PID.err = PID.SetSpeed - PID.ActualSpeed;    PID.PID_increase = PID.Kp*(PID.err-PID.Last_err) + PID.Ki*PID.err + PID.Kd*(PID.err-2*PID.Last_err+PID.Up_err);    PID.ActualSpeed += PID.PID_increase;    PID.Last_err = PID.err;    PID.Up_err = PID.Last_err;    return PID.ActualSpeed;}