kOS pid loop

//PID LOOP BASED ON THE github.com EXAMPLE.

SET run1 TO 1.

ON AG1 {
	SET run1 TO 0.
	PRINT "DONE".
	PRESERVE.
}

LOCK P TO 50 - ALT:RADAR. //ERROR

SET I TO 0.  //INTRODUCING THE "I" VARIABLE.
SET D TO 0.  //INTRODUCING THE "D" VARIABLE.
SET P0 TO P. //DEFINING THE 2ND ERROR VARIABLE.

//PID GAINS:
SET Kp TO 0.01.
SET Ki TO 0.1.
SET Kd TO 0.1.

LOCK dthrott TO Kp * P + Ki * I + Kd * D. //STANDARD PID CONTROLLER FORMULA


SET thrott TO 1.         //INTRODUCING THE "thrott" VARIABLE.
LOCK THROTTLE to thrott. //LOCKING THE THROTTLE TO "thrott" SO THAT IT CAN BE MANIPULATED LATER ON.

UNTIL run1 = 0 {   //IMPLEMETING THE 'RUNMODE' IDEA AND CEATING THE LOOP.
	SET dt TO 0.1. //SIMPLIFYING THE dt VALUE BY DEFINING A SET VALUE.
    IF dt > 0 {    //IMPLEMENTING ANOTHER LOOP.
        SET I TO I + P * dt.     //DEFAULT "I" VALUE FOR PID SCRIPT.
        SET D TO (P - P0) / dt.  //DEFAULT "D" VALUE FOR PID SCRIPT.

        IF Ki > 0 {
           SET I TO MIN(1.0/Ki, MAX(-1.0/Ki, I)).  //FILTER FOR "I" VALUE.
        }

        SET thrott to MIN(1, MAX(0, thrott + dthrott)). //LIMITING THROTTLE TO 0-1.
		SET P0 TO P.                                    //RETURNING THE 2ND ERROR VALUE.
        }
	PRINT "RDR ALT:" + ALT:RADAR AT (0,10).  //PRINTING THE TRUE RADAR ALTITUDE
	PRINT "ERROR  :" + p AT (0,12).          //PRINTING THE ERROR VALUE
}