LIB_PID

//=====Generic PID controller=====
//~~LIB_PID.ks~~
//~~Version 0.1~~

@LAZYGLOBAL off.

DECLARE FUNCTION PID_Init {

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

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

    DECLARE 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 {
    DECLARE PARAMETER
        PID_Array,      //Array built with PID_Init; updated with each call to PID_Seek
        seekVal,        //Desired value
        curVal.         //Current value

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

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

    DECLARE T TO TIME:SECONDS.
    DECLARE 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