PID self balancing (?) boe-bot and BS2 math

bulkhead · 2007-07-25 03:22

I'm trying to get a boe-bot to balance on 2 wheels using just a QTI line sensor. So far it only uses P and some D control to get the QTI's readings within the "setPoint" reading, which is the first reading taken when the boe-bot is powered on. That means if the first reading doesn't reflect a perfectly balanced boe-bot, then it will "drift" in the one direction that it was biased to at the start. Right now I've added a section to the code so that the "setPoint" value can be changed dynamically, but it isn't working that well (if at all). I have a few questions, with my main loop code posted afterwards:

1) Is there enough data just from the sensor alone to have the boe-bot find its own balance position?
1a) If so, is there anything wrong with my condition/algorithm for balance (2nd block of code down)

2) Is there an easier way of doing math with negative numbers with the BS2? I tried this:

IF (sign = 1) THEN error = -error

but when I

DEBUG DEC5 error

I get numbers like 63284 when they should be less than 2000. It's like the number would be negative if it were 2's complement, where the last bit is negative, but really it (that last bit) is positive in value so it makes a really large number. I don't think it is merely the formatting because the servos go full speed (as it the values they were given were actually really large, like 63284) when I did this. If I could simplify this, then I wouldn't need the nested IF statements in the last block and it would be a lot cleaner.

3)Any comments/suggestions on the PD part are welcome, this is my first attempt at PID control.


'p part
    error = setPoint - distanceReading
    sign = error.BIT15
    error = ABS(error)
    p = error/Kp            'Kp is a constant

    'changing setPoint (?) for self balancing
    'if current reading is close to set point AND last reading and current reading are significantly different
    IF(ABS (setPoint-lastDistanceReading)<10 AND ABS(distanceReading- lastDistanceReading)> 20) THEN 'within
       IF sign = 0 THEN setPoint = setPoint + error/20  'new reading smaller (leaning forward), so set point is further back
       IF sign = 1 THEN setPoint = setPoint - error/20  'new reading is larger (falling backward), so set point is further forward
       DEBUG "setpoint:", DEC5 setPoint,CR
    ENDIF

    'd part
    IF counter = 2 THEN   ' (calculate this once ever 4 loops)
      d = distanceReading - lastDistanceReading
      sign2 = d.BIT15
      d = (ABS d)/Kd        'Kd is a constant
      'd=0
      'DEBUG "d:",DEC d
    ENDIF  

    'actual servo updating part:
    IF(sign=0) THEN            'leaning forward, so drive forward
      IF (sign2=0) THEN               'accelearting back
        PULSOUT leftServo, centerValue+p-d
        PULSOUT rightServo, centerValue-p+d
      ENDIF
      IF (sign2=1) THEN
        PULSOUT leftServo, centerValue+p+d
        PULSOUT rightServo, centerValue-p-d
      ENDIF
    ENDIF
    IF(sign=1) THEN
      IF (sign2=0) THEN               'accelearting back
        PULSOUT leftServo, centerValue-p -d
        PULSOUT rightServo, centerValue+p +d
      ENDIF
      IF (sign2=1) THEN
        PULSOUT leftServo, centerValue-p  +d
        PULSOUT rightServo, centerValue+p -d
      ENDIF
    ENDIF
   lastDistanceReading = distanceReading

Post Edited (bulkhead) : 7/25/2007 3:30:54 AM GMT

Bruce Bates · 2007-07-25 07:42

bulkhead -

I suspect you're going to need an accelerometer to determine the degree of tilt, and whether it's forwards or backwards. That's what most balancing platforms use.

Regards,

Bruce Bates

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔

D Faust · 2007-07-25 13:22

Bulkhead, it might work better if you added a second QTI sensor on the other side, then add some math to make them read similar readings for similar differences.· That way you would tell when the bot isl balanced by seeign if the readings are the same.· Hope this helps.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
D Faust

PID self balancing (?) boe-bot and BS2 math

Comments