Dear all,
I have bitten the bullet and tried to go the signed numbers route on my line follower steering PID calculation. The bullet is bigger than it might look - I had to migrate to Shwindows to get #macro commands working and without them it would be even more confusing than the case statements I had difficulty to work through. Now it all looks beautiful and clear, but it does not work as expected and I cannot find the bug. Judging from the operation of the vehicle, it could be as little as one sign somewhere. I suspect it is the very beginning of the code, where I convert line_pos, which is a number between 1 and 7 for negative values, 8 and 9 for zero and 10 to 16 for positive values to ELine or error. I cannot figure out a solution, though.
For simplicity, I got rid of all the scaling as this introduced another layer of things I did not understand. Now I don't even need division, so it is multiplication only.
Here are my macro routines:
Here is the PID calculation loop:
Thank you for your input,
Edmunds
I have bitten the bullet and tried to go the signed numbers route on my line follower steering PID calculation. The bullet is bigger than it might look - I had to migrate to Shwindows to get #macro commands working and without them it would be even more confusing than the case statements I had difficulty to work through. Now it all looks beautiful and clear, but it does not work as expected and I cannot find the bug. Judging from the operation of the vehicle, it could be as little as one sign somewhere. I suspect it is the very beginning of the code, where I convert line_pos, which is a number between 1 and 7 for negative values, 8 and 9 for zero and 10 to 16 for positive values to ELine or error. I cannot figure out a solution, though.
For simplicity, I got rid of all the scaling as this introduced another layer of things I did not understand. Now I don't even need division, so it is multiplication only.
Here are my macro routines:
Code:
#macro MultiplySigned(word1,word2)
sign = positive
if word1 >= $8000 then
sign = NOT sign
word1 = -word1
endif
if word2 >= $8000 then
sign = NOT sign
word2 = -word2
endif
Result = word1 * word2
if sign = negative then
Result = -Result
endif
#endmacro
#macro SignedICap(word1)
if word1 >= $8000 then
ICap = -ICap
word1 = word1 MIN ICap
else
word1 = word1 MAX ICap
endif
Result = word1
#endmacro
Code:
KpELine = 20 'P coefficient/multiplier for line position error correction, 0.0x
KiEline = 5 'I coefficient for line error corrections, 0.00x
KdEline = 40 'D coefficient for line error corrections, 0.0x
ICap = 100
select case line_pos
case < 8
Eline = 8 - line_pos
Eline = NOT Eline
case > 9
Eline = line_pos - 9
else
ELine = 0 'Set error to zero
agr_error = 0 'Set accumulator to zero
steer_input = 0 'Do not steer
endselect
MultiplySigned(ELine,KpELine) 'Multiply error with P term coefficient
PLine = Result 'Put calculation into P term
agr_error = agr_error + ELine 'Ad up error for I term
MultiplySigned(agr_error,KiELine) 'Multiply accumulated error with I term coefficient
ILine = Result 'Put calculation into I term
SignedICap(ILine) 'Cap signed ILine at ICap magnitude
DLine = OldELine - ELine 'Calculate error trend
MultiplySigned(DLine,KdEline) 'Multiply error trend with D term coefficient
DLine = Result 'Put calculation into D term
steer_input = PLine + ILine + DLine 'Add everything up into a steering input
if steer_input bit 15 set then
low DIR
steer_input = NOT steer_input + 1
else
high DIR
endif
high EN 'Enable the device to steer, disable to save power and limit heat
for Counter = 0 to steer_input 'Increase the number of loops to steer 'more' per cycle or decrease for 'less'
pulsout STCK, 5000 'About a minimum to get decent power
pauseus 2500 'pauseus is double the length of pulsout, so half of pulsout time for a square wave
next
low EN
OldELine = ELine 'Set the old error the current error
Edmunds
Last edited: