PID control help.

Evil Axis · 2006-11-21 01:40

Hello,

I am working with the IR sensors and the boebot. I did the exercises in the book, and then i got interested in combatting the jitter as much as possible.

I tried setting up the sensors as suggested here: http://forums.parallax.com/showthread.php?p=561496
That helped, but the bot was still jittery.

Then I tried averaging out the sensor input(code in post referenced above). That slowed the jitter considerably, but I still wanted more.

Andy suggested that I try using PID control as per his excellent write-up: http://forums.parallax.com/showthread.php?p=529609

So now I am working on that, but I am having trouble getting it to work.

I think that there is a problem with my attempts at limiting the signal range that gets sent to the servos with the MIN 650 MAX 850 commands

What happens is that the program gives the expected output for a bit, and then sends the opposite of what would be expected. For example the left servo will get 650 for a bit, and then it will suddenly switch to 850 even though it should continue at 650 based on sensor input.

Here is the code:

' -----[noparse][[/noparse] Title ]--------------------------------------------------------------
' FollowingBoeBotWithNewDistanceCircuit.bs2
' Boe-Bot adjusts its position to keep objects it detects in zone 4.

' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

' -----[noparse][[/noparse] Constants ]----------------------------------------------------------

SetPoint       CON     4
CenterPulse    CON     750
Kp             CON     20                     ' Proportionality constant
Ki             CON     20                     ' Integral constant
Kd             CON     20                     ' Derivative constant
Current        CON     0                     ' Array index - current error
Accumulator    CON     1                     ' Array index - accumulated error
Previous       CON     2                     ' Array index - previous error
Delta          CON     3                     ' Array index - change in error

' -----[noparse][[/noparse] Variables ]----------------------------------------------------------
errorLeft      VAR     Word(4)               ' Four different types of errors
errorRight     VAR     Word(4)

irDetectLeft   VAR     Bit
irDetectRight  VAR     Bit
distanceLeft   VAR     Nib
distanceRight  VAR     Nib
pulseLeft      VAR     Word
pulseRight     VAR     Word

' -----[noparse][[/noparse] Initialization ]-----------------------------------------------------

FREQOUT 1, 500, 3000
DEBUG "er,dist,p,i,d,Pulse,er(acc),er,dist,p,i,d,Pulse,er(acc)", CR

' -----[noparse][[/noparse] Main Routine ]-------------------------------------------------------

DO
  GOSUB Get_Ir_Distances
  GOSUB Calculate_Drive
  GOSUB Send_Pulse
LOOP

' -----[noparse][[/noparse] Subroutine - Get IR Distances ]--------------------------------------

Get_Ir_Distances:

  distanceLeft = 0
  distanceRight = 0

  FOR DIRB = 1 TO 7

    FREQOUT 7,1,38500            'left ir led
    irDetectLeft = IN8
    distanceLeft = distanceLeft + irDetectLeft

    FREQOUT 3,1,38500           'right ir led
    irDetectRight = IN2
    distanceRight = distanceRight + irDetectRight

  NEXT

  RETURN

' -----[noparse][[/noparse] Subroutine - Calculate Drive ]----------------------------------------

Calculate_Drive:
  'Calculate left error / proportional
  errorLeft(Current) = SetPoint - distanceLeft
  'integral
  errorLeft(Accumulator) = errorLeft(Accumulator) + errorLeft(Current)
  'derivative
  errorLeft(Delta) = errorLeft(Current) - errorLeft(Previous)
  ' set previous
  errorLeft(Previous) = errorLeft(Current)

  ' Calculate output.
  pulseLeft = CenterPulse - ((Kp * errorLeft(current)) + (Ki * errorLeft(Accumulator)) + (Kd * errorLeft(delta))) MIN 650 MAX 850
  DEBUG SDEC errorLeft(Current), ",", DEC1 distanceLeft, ",", SDEC Kp * errorLeft(current), ","
  DEBUG SDEC Ki * errorLeft(Accumulator), ",", SDEC Kd * errorLeft(delta), ",", SDEC pulseLeft, ","
  DEBUG SDEC errorLeft(Accumulator), ","

  ' Calculate right error / proportional
  errorRight(Current) = SetPoint - distanceRight
  'integral
  errorRight(Accumulator) = errorRight(Accumulator) + errorRight(Current)
  'derivative
  errorRight(Delta) = errorRight(Current) - errorRight(Previous)
  'set previous
  errorRight(Previous) = errorRight(Current)

  'Calculate output.
  pulseRight = CenterPulse + ((Kp * errorRight(current)) + (Ki * errorRight(Accumulator)) + (Kd * errorRight(delta))) MIN 650 MAX 850
  DEBUG SDEC errorRight(Current), ",", DEC1 distanceRight, ",", SDEC Kp * errorRight(current), ","
  DEBUG SDEC Ki * errorRight(Accumulator), ",", SDEC Kd * errorRight(delta), ",", SDEC pulseRight, ","
  DEBUG SDEC errorRight(Accumulator), CR, CRSRX, 0

  RETURN


' -----[noparse][[/noparse] Subroutine - Send Pulse ]---------------------------------------------

Send_Pulse:
  PULSOUT 13,pulseLeft
  PULSOUT 12,pulseRight
  PAUSE 5
  RETURN

I have also attached a csv file (.txt format) that has data generated by the program.

Zoot · 2006-11-21 16:36

You're getting "windup" on your accumulated error and your K constants are probably too big. If you look at your data, the accumulated errors plus the multipliers on the K consants give you huge numbers (compared to your servo pulsouts) which won't limit properly.

For example, if I add a final calculated output of -1300 to 650, I get -650 --- but this won't MIN to 650 it will MAX to 850 because MIN and MAX work on variables like they were odometers -- if you roll them backwards they start counting from the highest number down again. If you MIN/MAX them at that point, you'll get the MAX because the number will effectively be very high.

Basically, MIN MAX will not work properly on negative values, so you need to account for that possibility either through logic or math.

There are tricks around this, the most common but not the fastest code wise is:

Sign VAR Bit         'for saving sign of signed word
work   VAR  Word  'some variable to multiple by "mult"; work might be negative or positive
mult   VAR   Word  'some multiplier  (might be a constant or PID output or whatever)

'this works for *, /, */ and **

Sign = work.BIT15  'save the "sign" bit -- Bit 15 will be 1 if the word is negative, 0 if the word is positive
work = ABS ( work ) ** mult   ' use absolute value of work -- the value without the sign; multiply it
IF Sign = 1 THEN work = -work   ' if work was negative to start, restore the negative sign

There are other ways too; see Tracy Allen's site for more discussion. In your case, you would need to check sign of your output before adding to the centerpulse and limiting. You might also need to limit the sheer output of the I portion of your controller ("integral windup" = the accumulated error goes to infinity):

Sign = output.BIT15          'save sign
output = ABS(output)         'limit output to centerpulse value
output = output MAX centerpulse    'this will give you minimum of 0 and max of centerpulse*2 when you add output to centerpulse
IF Sign = 1 THEN output = -output   'restore sign (motor direction)
motorspeed = output + centerpulse  'add limited output to centerpulse speed

I would also try dialing your I K constant down to 0, your I D constant down to 0 and set just your P K constant so the bot behaves without huge oscillations. Then start dialing up the I K and D K until you get smooth response without jitter. You may also need to allow for K values of less than 1 (i.e., what if the ideal PK is .75?)

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

Post Edited (Zoot) : 11/21/2006 4:52:29 PM GMT

Evil Axis · 2006-11-21 17:02

Zoot,

thanks.
I had looked up MAX and MIN in the PBASIC reference, but it didn't help me figure out what was wrong. I still don't completely understand the math limitations, of the BS2, but I am getting better at it.

I will take a look at this this evening when I get a chance to sit down with the bot again. I forgot to mention in the original post that the constants were way too big. I left them that way since it gave me a quick way to see what was happening in the debug terminal. I will see if I can implement your solution. I hope I have enough RAM left to do it. As it was I think I only have space for one or two more WORD variables.

Also could you give me a link to Tracy Allen's site?

Zoot · 2006-11-21 17:10

A single BIT variable can come in very handy even for stuff that is not sign related.

Another way to put MAX MIN limitation --- this won't work:

X = -1300
X = X MIN 1000 MAX 1200
(now X = 1200)

Why? because MIN and MAX don't see the negative sign, and on a BS2, the "negative" is really just BIT15, which means if it goes to 1 (for a "negative" number) it is SEEN as very HIGH number.

So you need to get rid of the negative sign, limit it, then restore negative if it's applicable. ABS just takes the - sign off ( ABS(-1000) = 1000 and ABS(1000) = 1000 ).

X = -1300
IF X.BIT15 = 1 THEN 'bit15 = 1 means a negative, which is less than 0, so use the MIN!
X = 1000
ELSE
X = X MIN 1000 MAX 1200 'if it's positive, then limit normally
ENDIF
(now X = 1000)

Tracy Allen -- BS2 math guru -- http://emesystems.com -- http://emesystems.com/BS2index.htm#math

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

Evil Axis · 2006-11-28 13:02

Zoot,

thanks. I finally got back to this, and using BIT15 works nicely.

here is how I am doing the calculation now:

temp = CenterPulse - ((Kp * errorLeft(current)) + (Ki * errorLeft(Accumulator)) + (Kd * errorLeft(delta)))
IF temp.BIT15 = 1 THEN 'bit15 = 1 means a negative, which is less than 0, so use the MIN!
    pulseLeft = 650
ELSE
    pulseLeft = temp MIN 650 MAX 850 'if it's positive, then limit normally
ENDIF

temp is a Word

Also thanks for the Tracy Allen link. Great stuff there!

Zoot · 2006-11-28 14:59

Great! And yes, Dr. Allen is the man!

If you ever need fractional constants for your KP, KI and KD you can do it like this. Use K's that are in 100ths -- e.g. 100 would be 1, 150 would be 1.5, etc. Then multiply by 100 first, dividing back by 100 at the end:

Kp CON 200 'same as "2" since it's in 100ths
Ki CON 150 'same as "1.5" since it's in 100ths
Kd CON 020 'same as ".2" since it's in 100ths
temp = Kp * errorLeft(current) / 100
temp = temp + ( Ki * errorLeft(Accumulator) / 100 )
temp = temp + ( Kd * errorLeft(Delta) / 100 )

To be really precise, though add 50 before dividing (this will round .5 up and < .5 down):

temp = temp + ( Kd * errorLeft(Delta) + 50  / 100 )

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

Zoot · 2006-11-28 16:45

Oh, duh, if you use fractional constants, don't forget to account for sign before dividing out the 100...

Sign = errorLeft(Delta) >> 15  'you can't do a .BIT15 on array element
work = ABS( errorLeft(Delta) ) * Kd  / 100
IF Sign = 1 THEN work = -work
temp = temp + work

Sign = errorLeft(Accum) >> 15  'you can't do a .BIT15 on array element
work = ABS( errorLeft(Accum) ) * Ki  / 100
IF Sign = 1 THEN work = -work
temp = temp + work

'etc......

I wrote a PID engine so that setpoints, offsets, constants, etc. are stored in tables in EE and then a single set of subroutines runs 'em through the mill and delivers final output. If you are only doing one set of PID moves, then just writing it out is good, but if you have to run several motors, sensors, whatever, each through their own set of PID constants, it can be nice to have a PID calculator that does all the grunt work.

http://forums.parallax.com/attachment.php?attachmentid=42031

There's a lot of I/O and variable declarations in the code that you can skip -- the pertinent part is the EE lookup tables for PID constants, offsets, direction, etc. and the routines that call the actual PID sub -- scroll down the code and see IRbPID, for example -- an EE addr for the data is set, an SPRAM address is set for retrieving the sensor data, then it runs through the mill. You could easily change the routines to use regular variables instead of retrieved scratch pad variables. In any case, GOSUB PID and it's associated routines are what do the bulk of the work; these should be clear enough. This is actually older code, my current engine is more compact, but is harder to read and doesn't have many comments.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST

PID control help.

Comments