LIB_PID.ks

//=====Generic PID controller=====
//~~LIB_PID.ks~~
//~~Version 0.9b~~

@LAZYGLOBAL OFF.

DECLARE FUNCTION PID_Init {

    PARAMETER
        Kp,     //Gain of position
        Ki,     //Gain of integral
        Kd.     //Gain of derivative

    LOCAL SeekP TO 0.       //Desired value for P (will get set later)
    LOCAL P TO 0.           //Proportional term
    LOCAL I TO 0.           //Integral term
    LOCAL D TO 0.           //Derivative term
    LOCAL oldT TO -1.       //Old Time; start value shows that it hasn't been calculated
    LOCAL oldInput TO 0.    //Previous return value of controller

    GLOBAL PID_Array TO LIST().
    PID_Array:ADD(Kp).          //[0]
    PID_Array:ADD(Ki).          //[1]
    PID_Array:ADD(Kd).          //[2]
    PID_Array:ADD(SeekP).       //[3]
    PID_Array:ADD(P).           //[4]
    PID_Array:ADD(I).           //[5]
    PID_Array:ADD(D).           //[6]
    PID_Array:ADD(oldT).        //[7]
    PID_Array:ADD(oldInput).    //[8]

    RETURN PID_Array.

}. //FUNCTION PID_Init

DECLARE FUNCTION PID_Seek {
    PARAMETER
        PID_Array,      //Array built with PID_Init; updated with each call to PID_Seek
        seekVal,        //Desired value
        curVal.         //Current value

    LOCAL Kp TO PID_Array[0].
    LOCAL Ki TO PID_Array[1].
    LOCAL Kd TO PID_Array[2].
    LOCAL oldS TO PID_Array[3].
    LOCAL oldP TO PID_Array[4].
    LOCAL oldI TO PID_Array[5].
    LOCAL oldD TO PID_Array[6].
    LOCAL oldT TO PID_Array[7].
    LOCAL oldInput TO PID_Array[8].

    LOCAL P TO seekVal - curVal.
    LOCAL I TO 0.
    LOCAL D TO 0.
    LOCAL newInput TO oldInput.

    LOCAL T TO TIME:SECONDS.
    LOCAL dT TO T - oldT.

    IF oldT < 0 {
        //Nothing
    } ELSE {
        IF dT = 0 {
            SET newInput TO oldInput.
        } ELSE {
            SET D TO (P - oldP) / dT.
            SET I TO (oldI + P) * dT.
            SET newInput TO Kp * P + Ki * I + Kd * D.
        }.
    }.

    SET PID_Array[3] TO seekVal.
    SET PID_Array[4] TO P.
    SET PID_Array[5] TO I.
    SET PID_Array[6] TO D.
    SET PID_Array[7] TO T.
    SET PID_Array[8] TO newInput.

    RETURN newInput.

}. //FUNCTION PID_Seek