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

Chapter 5

‘
‘Since there is no error derivative term, we will use
‘Dgain to mean the rabbit derivative gain. Its gain
‘depends on the polarity of the rabbit derivative.
If RabbitDeriv > 0 Then
DGain = ControlSingles(DposGain)
Else
DGain = ControlSingles(DnegGain)
End If

‘Now do the calculation for each term.
‘First limit the error to the band for each gain
If Error > ControlSingles(PBand) Then
LimError = ControlSingles(PBand)
ElseIf Error < -ControlSingles(PBand) Then
LimError = -ControlSingles(PBand)
Else
LimError = Error
End If
PTerm = CDbl((PGain * LimError) / 100)

If Error > ControlSingles(IBand) Then
LimError = ControlSingles(IBand)
ElseIf Error < -ControlSingles(IBand) Then
LimError = -ControlSingles(IBand)
Else
LimError = Error
End If

‘The I term is cumulative, so it’s gain range is 1/100th that of P.
‘Integral is bled off while rabbit is moving, or if the output
‘accumulator has gone below zero with a negative integral or over
‘full range with a positive integral.

If Abs(RabbitDeriv) < ControlSingles(AttackRate) / 30 Then
If (LimError > 0 And IntegralHold >= 0) Or _
(LimError < 0 And IntegralHold <= 0) Then
ITerm = LimitTerm(ITerm + ((IGain * LimError) / 10000))
End If
Else ‘Bleed off the i term.
ITerm = 0.99 * ITerm
End If

DTerm = LimitTerm(RabbitDeriv * DGain * 10)
RTerm = LimitTerm(TempRabbit * (RGain) / 500)
OutputAccum = PTerm + ITerm + DTerm + RTerm

68