Boe Bot based Inverted Pendulum

Cheeto · 2006-06-23 19:59

I built this using Boe-Bot parts and two QTI line followers. The chasis is a cardboard box I had laying around my desk and the whole thing is held together by electric tape. Right now it kind of works, it cans stay up for about 30-45 seconds on a good run, I think my main problem lays in the servos not being powerful or quick enough to compensate for the weight shift. I attached my code at the bottem if anyone else wants to take a stab at it.

'{$STAMP BS2}
'{$PBASIC 2.5}

pos1 VAR Word      'Pos from RCTime
pos1_old VAR Word  'Old posistion
pos2 VAR Word
pos2_old VAR Word
mag VAR Word      'Magnitude of ange
diff VAR Word     'Differnce from new and old new- old


center1 VAR Word    'Calculated straight up RC time factor
center2 VAR Word
counter VAR Nib

vel1 VAR Word
vel2 VAR Word
total VAR Word


'Calibration
HIGH 10
FOR counter = 0 TO 14
  HIGH 3
  HIGH 2
  RCTIME 3,1, vel1
  RCTIME 2,1, vel2
  center1 = vel1 + center1
  center2 = vel2 + center2
  PAUSE 50
NEXT
LOW 10
  center1 = center1 /15
  center2 = center2 /15


pos1 = center1 'Center
pos2 = center2

main:
   pos1_old = pos1  'Set last posistion taken to
   pos2_old = pos2
   HIGH 3       'set pin 3 high in prep to run rctime
   HIGH 2 
   RCTIME 3,1, pos1
   RCTIME 2,1, pos2

   IF ( pos1 < center1 ) THEN      'Need to go to Fwd
      mag = ABS(center2-pos2)
      diff = ABS(pos2_old-pos2)
      total = ((mag)+(diff))*4 MAX 150 MIN 0
      GOTO Fwd
   ELSEIF ( pos2 < center2 ) THEN      'Need to go to reverse
      mag = ABS(center1-pos1)
      diff = ABS(pos1_old-pos1)
      total = ((mag)+(diff))*4 MAX 150 MIN 0
      GOTO Rwd
   ELSE
    GOTO main
   ENDIF
GOTO main

RWD:
vel1 = total + 750
vel2 = 750 - total
GOTO ServoC

FWD:
vel1 = 750 - total
vel2 = total + 750
GOTO ServoC

ServoC:
  PULSOUT 13,vel1
  PULSOUT 12,vel2

GOTO main

Post Edited (Cheeto) : 6/23/2006 8:03:14 PM GMT

Franklin · 2006-06-23 21:14

You might save a bit of response time if you put the pulsouts in the fwd and rwd actions:
FWD:
PULSOUT 13, 750 - total
PULSOUT 12, total + 750
RETURN

(I think that will work)

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
- Stephen

Cheeto · 2006-06-23 21:24

Using the RCTIME command twice I get a center reading on each sensor of around 250 which means its 500uS a piece or 1mS total just to read in each sensor. I was thinking of attempting to use smaller caps to drop the center RCTIME around 100 which would be 200uS and 400uS total. I just need to find some smaller caps to try it out with.

Now that you mention moving the pulsout, I may just get rid of the subroutines and throw all the commands in the respective IF statements.

Beau Schwabe · 2006-06-23 22:04

If you can alter your sensor approach so that you are using COUNT, you might get better results. Since RCTIME is time variant, as the sensor value changes, so does the time it takes to iterate through your loop
which can add to a "jerky" motion. Using COUNT would allow you to at least set a specific time window that will keep your loop iterations more predictable. To do this you need to feed your sensor into a 555 or
74HC14 Schmitt trigger type of oscillator. The output of the oscillator will be a continuous stream of pulses that can be measured with the COUNT command.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Beau Schwabe

IC Layout Engineer
Parallax, Inc.

Boe Bot based Inverted Pendulum

Comments