Option explicit
'
' Array positions for PID parameters and working variables
'
Const Kp=0
Const Ki=1
Const Kd=2
Const tau=3
Const limMin=4
Const limMax=5
Const limMinInt=6
Const limMaxInt=7
Const T=8
Const integrator=9
Const prevError=10
Const differentiator=11
Const prevMeasurement=12
Const out=13
'
Dim pid_params(13)
'
pid_params(Kp)=2.0
pid_params(Ki)=0.5
pid_params(Kd)=0.25

' Derivative low-pass filter time constant
pid_params(tau)=0.02

'Output limits
pid_params(limMin)=-10.0
pid_params(limMax)=10.0

'Integrator limits
pid_params(limMinInt)=-5.0
pid_params(limMaxInt)=5.0

'Sample time (in seconds)
pid_params(T)=0.01

'Controller "memory"
pid_params(integrator)=0.0
pid_params(prevError)=0.0 'Required for integrator'
pid_params(differentiator)=0.0
pid_params(prevMeasurement)=0.0 'Required for differentiator

'Controller output
pid_params(out)=0.0


Dim float setpoint=1.0
Dim integer near_enough=0
Timer =0
'
' Initialise a PID loop with a 10mSec loop time
'
Math PID INIT 1,pid_params(),pidint
'
' Start the PID controller
'
Math pid start 1
Do
Loop Until near_enough
Print "test over"
Math pid stop 1
Math pid close 1
End

'
'Subroutine called every 10mSec
'
Sub pidint
 Local float measurement=test(pid_params(out)) 'simulate an input measurement
 Local float output=Math(PID 1,setpoint,measurement) 'update the controller
 Print Int(Timer),Str$(measurement,8,5),Str$(output,8,5)
 If Abs(measurement-setpoint)<0.00001 Then near_enough=1
End Sub

Function test(inp As float) As float
 Static float output=0.0
 Const alpha=0.02
 output=(pid_params(T)*inp + output)/(1.0 + alpha * pid_params(T))
 test=output
End Function