Page 84 - Designing Autonomous Mobile Robots : Inside the Mindo f an Intellegent Machine

Closed Loop Controls, Rabbits and Hounds

Figure 5.5 shows typical code for a thermal PID control using many of the terms and
tricks just discussed.

‘Calculate the PID for a single control using error proportional,
‘error integral, rabbit, and rabbit derivative terms. Error and
‘rabbit derivative gains are non-symmetric.

‘The process is controlled by an array of singles called
‘“ControlSingles”. Each term also has a limit band on its error.
‘If the error is greater than the band, then the band value is
‘substituted for the limit.

‘Routine returns the power command as an integer between 0 and 9999.
Public Static Function DoPIDs(TempRabbit As Single, Temp As Single) As Integer

Dim Error As Single ‘Raw error
Dim LimError As Single ‘Error or limit, whichever is smaller.
Dim OutputAccum As Long
Dim PGain As Long ‘Proportional command Gain (0 to 9999)
Dim DGain As Long ‘Derivative command Gain
Dim IGain As Long ‘Integral command Gain
Dim RGain As Long ‘Setpoint rabbit command Gain
Dim PTerm As Single
Dim ITerm As Single
Dim DTerm As Single
Dim RTerm As Single
Dim RabbitDeriv As Single
Dim LastTempRabbit As Single
Dim IntegralHold(MaxZones) As Integer ‘0=No Hold, 1=allow pos. only, -1=allow neg.

On Error Resume Next

‘Calculate the error and rabbit derivative.
Error = TempRabbit - Temp
RabbitDeriv = TempRabbit - LastTempRabbit
LastTempRabbit = TempRabbit
‘Get the Rabbit and error gains
RGain = ControlSingles(RabbitGain) ‘Rabbit gain is always positive.

‘Some gains depend on error polarity
If Error >= 0 Then ‘For positive errors use positive gains
PGain = ControlSingles(PposGain)
IGain = ControlSingles(IposGain)
Else
PGain = ControlSingles(PnegGain)
IGain = ControlSingles(InegGain)
End If

67