88 lines
3.1 KiB
Plaintext
88 lines
3.1 KiB
Plaintext
// lib_pid.ks provides routines to implement a simple generic PID controller.
|
|
// Copyright © 2015,2016,2019,2023 KSLib team
|
|
// Lic. MIT
|
|
@LAZYGLOBAL off.
|
|
@CLOBBERBUILTINS off.
|
|
|
|
HUDTEXT("lib_pid.ks has been superseded by the kOS inbuilt PIDloop() function.", 10, 2, 30, RED, FALSE).
|
|
wait 0.5.
|
|
HUDTEXT("It is maintained for example purposes only, please see the kOS documentation for more.", 10, 4, 30, RED, FALSE).
|
|
|
|
function PID_init {
|
|
parameter
|
|
Kp, // gain of position
|
|
Ki, // gain of integral
|
|
Kd, // gain of derivative
|
|
cMin, // the bottom limit of the control range (to protect against integral windup)
|
|
cMax. // the the upper limit of the control range (to protect against integral windup)
|
|
|
|
local SeekP is 0. // desired value for P (will get set later).
|
|
local P is 0. // phenomenon P being affected.
|
|
local I is 0. // crude approximation of Integral of P.
|
|
local D is 0. // crude approximation of Derivative of P.
|
|
local oldT is -1. // (old time) start value flags the fact that it hasn't been calculated
|
|
local oldInput is 0. // previous return value of PID controller.
|
|
|
|
// Because we don't have proper user structures in kOS (yet?)
|
|
// I'll store the PID tracking values in a list like so:
|
|
//
|
|
local PID_array is list(Kp, Ki, Kd, cMin, cMax, SeekP, P, I, D, oldT, oldInput).
|
|
|
|
return PID_array.
|
|
}.
|
|
|
|
function PID_seek {
|
|
parameter
|
|
PID_array, // array built with PID_init.
|
|
seekVal, // value we want.
|
|
curVal. // value we currently have.
|
|
|
|
// Using LIST() as a poor-man's struct.
|
|
|
|
local Kp is PID_array[0].
|
|
local Ki is PID_array[1].
|
|
local Kd is PID_array[2].
|
|
local cMin is PID_array[3].
|
|
local cMax is PID_array[4].
|
|
local oldS is PID_array[5].
|
|
local oldP is PID_array[6].
|
|
local oldI is PID_array[7].
|
|
local oldD is PID_array[8].
|
|
local oldT is PID_array[9]. // Old Time
|
|
local oldInput is PID_array[10]. // prev return value, just in case we have to do nothing and return it again.
|
|
|
|
local P is seekVal - curVal.
|
|
local D is oldD. // default if we do no work this time.
|
|
local I is oldI. // default if we do no work this time.
|
|
local newInput is oldInput. // default if we do no work this time.
|
|
|
|
local t is time:seconds.
|
|
local dT is t - oldT.
|
|
|
|
if oldT < 0 {
|
|
// I have never been called yet - so don't trust any
|
|
// of the settings yet.
|
|
} else {
|
|
if dT > 0 { // Do nothing if no physics tick has passed from prev call to now.
|
|
set D to (P - oldP)/dT. // crude fake derivative of P
|
|
local onlyPD is Kp*P + Kd*D.
|
|
if (oldI > 0 or onlyPD > cMin) and (oldI < 0 or onlyPD < cMax) { // only do the I turm when within the control range
|
|
set I to oldI + P*dT. // crude fake integral of P
|
|
}.
|
|
set newInput to onlyPD + Ki*I.
|
|
}.
|
|
}.
|
|
|
|
set newInput to max(cMin,min(cMax,newInput)).
|
|
|
|
// remember old values for next time.
|
|
set PID_array[5] to seekVal.
|
|
set PID_array[6] to P.
|
|
set PID_array[7] to I.
|
|
set PID_array[8] to D.
|
|
set PID_array[9] to t.
|
|
set PID_array[10] to newInput.
|
|
|
|
return newInput.
|
|
}.
|