python 实现 增量型PID

class _pid:
    def __init__(self,setSpeed,actualSpeed,err,err_next,err_last,Kp,Ki,Kd):#,voltage,intergral):
        self.setSpeed = setSpeed;
        self.actualSpeed = actualSpeed;
        self.err = err;
        self.err_next = err_next;
        self.err_last = err_last;
        self.Kp = Kp;
        self.Ki = Ki;
        self.Kd = Kd;
        #self.voltage = voltage;
        #self.intergral = intergral;



def PID_init():
    pass;

def PID_realize(speed):
    pID.setSpeed = speed;
    pID.err = pID.setSpeed-pID.actualSpeed;
    #pID.voltage = pID.Kp * pID.err  + pID.Ki * pID.intergral + pID.Kd * (pID.err-pID.err_last);
    incrementSpeed = pID.Kp *(pID.err-pID.err_next) + pID.Ki*pID.err + pID.Kd * (pID.err-2*pID.err_next +pID.err_last);
    pID.actualSpeed = pID.actualSpeed + incrementSpeed;
    pID.err_last = pID.err_next;
    pID.err_next = pID.err;
    return pID.actualSpeed;




# Press the green button in the gutter to run the script.
if __name__ == '__main__':
    pID = _pid(0.0, 0.0, 0.0,0.0, 0.0, 0.2, 0.015, 0.2);#, 0.0, 0.0);


    count = 0;
    while(count<450):
        speed = PID_realize(200.0);
        count = count+1;
        print(speed)