Balancing Robot Using Ping

JohnBF

My goal is to build a balancing robot that can roam and avoid obstacles. As a first step I built a simpler robot that can balance in place or move slowly forward or backward over a level surface.
·
I posted a video at
·
I used mostly components from my last Boe-Bot project: Parallax continuous rotation servos, a BOE development board with a BS2sx chip and Ping to measure tilt angle. The balance point can be moved slightly fore or aft with on-board buttons to choose stationary or slowly moving behavior and to fine tune for different environments. The chassis is made from Radio Shack perf board. For power I’m using five Ni-CDs, whose low internal resistance provides quicker surges as the servos constantly start, stop, and change direction. The only non-Parallax working parts are 3” model airplane wheels.

I chose the Ping as tilt sensor after tests showed it to be the most accurate of the sensors I had on hand from previous projects. I collected data·by running the robot with the serial cable attached, holding the cable to minimize it's affect on balance. Not elegant, but it worked.
·
The closed feedback loop is based on Andy Lindsay’s wonderfully clear explanation·of how to implement a PID loop on a Basic Stamp. For this application I found that for the integral term, rather than maintaining a running total, ·I got better results with a leaky accumulator which remembers only the previous two errors.
·
I’m eager to start a more sophisticated balancing robot and·I’d appreciate any ideas or advice:
·
I’d like to go with a gyro/accelerometer combination. I already have a Memsic 2125. What’s the best choice for gyro? Perhaps the ADXRS300 on a breakout board from SparkFun.com? Does anyone have ideas or leads on information about algorithms to utilize these sensors on a Basic Stamp or SX chip (preferably in SX/B)?
·
I’d like to use DC motors. Any suggestions on motors that have a little more torque, a little more speed, and a little less latency than hobby servos?·
·
Thanks!
·
Below is the latest version of the pBasic code.
·
/John
·
·
' {$STAMP BS2sx}
' {$PBASIC 2.5}
'
' Balancer.BS2X. A simple two wheel balancing robot using a
' Ping))) sensor for tilt angle.
'
' John Fisher, January 2007
'
'

---

[noparse][[/noparse] I/O DEFINITIONS ]

---

· buttonBottom······ PIN···· 10
· buttonTop········· PIN···· 11
· rwheel············ PIN···· 12
· lwheel············ PIN···· 13
· ping·············· PIN···· 14
· speaker··········· PIN···· 15
·
'

---

[noparse][[/noparse] CONSTANTS/PARAMETERS ]

---

' PID contstnts:
· Kp··········· ·····CON······ 2····
· Ki················ CON······ 2····
· Kd················ CON······ 4····
·
'Meaningful names for error array index:
· Current··········· CON······ 0
· Sum··············· CON······ 1
· Last·············· CON······ 2
· SecondtoLast·· ····CON······ 3
· Delta············· CON······ 4
·
'

---

[noparse][[/noparse] VARIABLES ]

---

·
· pTime············· VAR···· Word··· ' raw value from Ping)))
· drive············· VAR···· Word··· ' value sent to Motors
················· ···················
· error············· VAR···· Word(5)
·
· p················· VAR···· Word··· ' proportional term
· i················· VAR···· Word··· ' integral term
· d················· VAR···· Word··· ' derivative term
·
· SetPoint······ ····VAR···· Word···
·
· counter··········· VAR···· Nib····
·
'

---

[noparse][[/noparse] INITIALIZATION ]

---

·
Reset:
GOSUB ProgramStartSound
SetPoint = 540
·
'

---

[noparse][[/noparse]PROGRAM CODE ]

---

·
Main:
·
IF buttonTop = 1 THEN
· SetPoint = SetPoint + 1
· FREQOUT speaker, 12, 1350··· ' confirm button push
· PAUSE 175
ENDIF
IF buttonBottom = 1 THEN
· SetPoint = SetPoint - 1·····
· FREQOUT speaker, 12, 1250··· ' confirm button push
· PAUSE 175
ENDIF
·
DO
·
·PULSOUT ping, 12
·PULSIN ping, 1, pTime
·IF (pTime<425)OR(pTime>625)THEN GOTO Main 'Shut down motors
·········································· 'if beyond recoverable
·········································· 'tilt angle.
·error(current) = setPoint - pTime
·p = error * Kp
·error(Sum)=error(Current)+error(Last)+error(SecondtoLast)
·i = Ki * error(Sum)
·error(Delta) = error(Current) - error(Last)
·d = Kd * error(Delta)
·drive =· p + d + i
·GOSUB Motors
·error(SecondtoLast) = error(Last)
·error(Last) = error(Current)
·
' Code to send runtime data to debug goes here. The
' Parallax oscilloscope was used to check the actual
' time between motor pulses so the pause in the motor routine
' could be adjusted to stay close to 20 ms. The main value
' of using the BS2sx was that the BS2 was not fast enough
' to read the sensor, calculate the motor input, send
' data to debug, and get back to the motors within 20 ms.
·
LOOP
END
·
'

---

[noparse][[/noparse] SUBROUTINES ]

---

·
ProgramStartSound:
· FOR counter = 4 TO 1
··· FREQOUT speaker, 30, (1320) - (counter * 36)
··· PAUSE 100
· NEXT
RETURN
·
Motors:
· PULSOUT lwheel, (1871 + drive) MIN 1625 MAX 2111
· PULSOUT rWheel, (1875 - drive) MIN 1625 MAX 2125
· PAUSE 3
RETURN

Forrest

That's very impressive. I would not have thought the Ping))) was accurate and responded fast enough to accomplish this feat. It's amazing you were able to accomplish a balancing rebot with such few components.

JohnBF

Thanks. I was surprised as well. I tested a variety of IR arrangements and the Ping was better, at least within a very narrow range of tilt angles. But the narrow range is all that's useful anyway.

The Ping reading only takes a few milliseconds. If you run them back to back strange things happen - probably the echoes are still bouncing around the walls. But there's no reason to update the readings faster than the 20 msecond servo pause, and at this rate everything works fine.

/John

Tronic (Greece)

Wow! It looks fine!

Have you tried yet to move it around?
Look on my simular project for code ideas to add to it a simple 4 channel RF control:

http://forums.parallax.com/showthread.php?p=570699


Regards, Thanos


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


Greekbotics: Greek Robotics Forum

(Translate it using Babelfish)
·

Beau Schwabe

JohnBF,

Impressive! .... what happens if you jingle your keys nearby?

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

IC Layout Engineer
Parallax, Inc.

AIman

Good work.

I have read over both yours and Tronics, you both have done good work.

JohnBF

Thanks, Thanos. I did look at your excellent project and it helped give me confidence to try one of my own. I will definitely try an RF solution to robot-world communcations.

Beau, I haven't tried jingling keys, but the Ping is definitely sensitive to ambient noise that gets into the ultrasonic range, maybe from harmonics. Forget Kalman filtiers - we need jingle filters for the Ping!

/John

Tricky Nekro

Nice looking... well done...

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
-Rule your Destiny-
--Be Good. Be Bad. Be Provas--

The hellinic (Greek) robots portal: Greekbotics
Many Projects and Schematics by the users·and also robotic news
(Translate using babelfish)

lboucher

Hey Everyone. Was inspired to try and make one of my own. The only extras I used were bigger wheels I had lying around and adding an extra battery pack to handle the current needs of the servos. Defintely found a lot of odd things happening to the bs2 and ping when the current is being pulled so hard. But with alot of tuning I finally got the thing to balance for over a minute. I believe that if I raise the batters up 3 or so inches the stability will greatly improve and will post again once I try this.

I included my dirty dirty code if anyones interested. Some math in the code does not really make sense. I think im getting errors due to current fluctuations but not sure. I need to go thru it when im not tired and really debug it.

And here is the video on you tube.

http://www.youtube.com/watch?v=9ZdlpufdbfI

Steve Joblin

Very cool! quick physics question... I assume you want to move the batteries up a few inches so that the center of mass is moved up... why? Isn't something that has it's center of mass lower to the ground usually considered to be more stable (thinking about SUV's vs sports cars flipping over, or how it is easier for a child to learn to ski than an adult because they are lower to the ground and inherintly more stable)?

Zoot

That's true if you are not moving or you have multiple points of balance -- if you are moving (i.e. balancing) then the center of gravity needs to be high -- this allows a greater range of motion below the center gravity before the center of gravity moves (i.e. topples over).

Think of an inverted pendulum. Or try this -- balance a shop broom by the handle (with one finger). Easy. You can move your arm quite a bit and the broom head hardly moves. Now try it with the handle in the air. Almost impossible. Or think of a unicyclist -- the rider can peddle the wheel back and forth over nearly two feet and his/her upper body may hardly move at all (relative to the ground).

Way cool 'bot. Post more video when you have it turning and such!!!

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

Steve Joblin

Thanks Zoot... amazing how when you get a picture in your head it all makes sense!

JohnBF

I'm not sure of the physics of this, but observing my bot suggests that raising the center of gravity and also distributing the weight further fore and aft both add stability within a recoverable range of tilt angles, but the trade-off is that they narrow the revoverable range of tilt angles. I think that both of the above changes add rotational inertia which resist disturbances, but once they get moving they make it harder for a given drive train to overcome before falling.

Larger wheels definitely help the bot to balance, but I'm not sure why. Maybe it's just that the larger wheels give the small servos more speed along the ground. Seems like more than that, but I don't know what.

Another question I can't get my arms around is this: when the motor makes the drive shaft and wheels turn, how does that effect the rest of the robot? I can't figure out whether a wheel turning clockwise generates a clockwise or counterclockwise force to the chassis. And are the two wheels generating the same force to the chassis, or opposite forces? In each configuration of my bot I found an unexpected turning tendency. Maybe this is because of the opposite rotation of the two wheels.

Sure wish I had paid more attention when I took physics in college.

/John

Zoot

Somebody said...
I'm not sure of the physics of this, but observing my bot suggests that raising the center of gravity and also distributing the weight further fore and aft both add stability within a recoverable range of tilt angles,

I think the fore and aft distribution will work against you unless the weight is brought down near the ground (like an inverted "U" braced across the top of your 'bot with weights at the bottom of the descenders). You will also have considerable inertia with that kind of setup (meaning more power & time to effect a result).

The larger wheels help mostly to give you greater speed in a shorter time -- i.e., faster corrections. There will be a inverse trade-off in power, of course. It is also possible that getting the pivot point of your balance a bit higher above the wheel's contact with the ground helps, but like you, my physics suffers at that point.

Your tendency to turn may be slight mismatches in servo speeds -- send the same pulse to both servos and you probably won't get exactly the same speed on either one. You might want to set up manual neutral/max/PID constants for EACH wheel to get them tuned (in the absence of real-world encoders, that is).

Have you seen the Bal-bot site? They have a very tall balancing 'bot with the battery weight way, way up high. Very large wheels, too, relative to the size of the 'bot.


www.balbots.com/product_info.php?cPath=21&products_id=28

I guess they had a fire and are not shipping product anymore, which is too bad.

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

lboucher

From what I have figured out about the physics of this.

The higher the center of mass is, the greater the damping effect on the whole system. THis is good because as can be seen in the video the servos are jerking around alot and causeing it to overshoot back and forth. So if I move the batteries higher it will not overshoot so bad and become more stable.

The other trade of is between height and the max speed of the servos. The servos have enough torque to do the job but top speed is an issue. Basicly there is a direct relationship between max motor speed and recoverable tilt angle. If I move the center of mass I restrict the recoveable angle for a given maximum motor speed.

This means the higher the center of mass the more important it is to have a very precise measure of tilt angle to drive the motors with. The way I have the bot set up right now the ping is very close to the center of the body and at an angle. I need to get rid of the angle and have it point straigt down when balanced so that the change in ping time will be equal in either tilt direction. And i need to move it out from the bode a few inches to get the additional precision needed to move the Center of mass uppward effectively.

Also anyone ever heard of the Grand Challenge. I was on the VA Tech Team.

lboucher

Finally got my bs2 controlled boe to be stable and balance on two wheels.



http://www.youtube.com/watch?v=vhJIWGOb8hg

JohnBF

Looks great! I guess part of the lesson here is that short of much more sophisticated arrangements, Ping))) is a pretty good sensor for a balancing robot. And very convenient -- the Parallax mounting bracket places it just the right distance forward.

Since the whole thing can be done with Parallax parts, maybe this could be a Boe-Bot/Ping))) AppNote. It's a really fun project and a surprising and impressive accomplishment for this collection of parts.

/John

D Faust

JohnBF,
I like your simplistic approach to the balancing bot problem. Seems to be a very popular project idea. Yours is one of the most vertical ones that I have seen, great job!!
What did you use for the frame, looks like some sort of plastic and hot glue? Is it light?

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

JohnBF

D. Faust,

Thanks! I made the frame from Radio Shack perf board, glued up with epoxy. I live in an apartment and my workshop is also the dining room table, and everything gets folded up and stowed in a closet after every·work session,·so I have no choice but to keep everything very simple.·Maybe that's a blessing in disguise.

In the middle of experimenting with various combinations of accelerometers and gyros, I got even more interested in this guitar tuner project: http://forums.parallax.com/showthread.php?p=641934

At the moment I'm getting ready for a trip to Germany and I hope to get back to both the tuner and·the balancing robot project in July.

Looking forward to seeing progress by others with balancing robots, particularly using combinations of prop/accelerometer/gyros.

/John

Whit

JohnBF,

Very cool. I am amazed how well it works.

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Whit+


"We keep moving forward, opening new doors, and doing new things, because we're curious and curiosity keeps leading us down new paths." - Walt Disney

Whit

JohnBF said...

Since the whole thing can be done with Parallax parts, maybe this could be a Boe-Bot/Ping))) AppNote. It's a really fun project and a surprising and impressive accomplishment for this collection of parts.

/John

I agree. lboucher's·BOE Bot version is pretty cool - and I think I·already have all the parts! Hey lboucher, how about some pictures of the configuration of your balancing bot?

▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Whit+


"We keep moving forward, opening new doors, and doing new things, because we're curious and curiosity keeps leading us down new paths." - Walt Disney

Post Edited (Whit) : 6/6/2007 4:55:08 AM GMT

markward

This is great stuff. I'm interested in trying this with a BS2, memsic 2125, and ir bumpers and pretty much a stock boebot. Looking for it to roam. Stumbling blocks?

markward

Man... this was a really enjoyable project. Code, picture and video attached. Not a whole lot new here, the design is pretty simplistic and v. similar to others. Code pasted below is courtesy of Andy Lindsey's many PID discussions and only slightly modified. Able to take advantage of his fractional multiplication to really fine tune the PID. All I can say is that industrial strength velcro and a glue gun are a prototypers best friend. One thing of note - I managed to get away with using only a BS2. You'll see a steel rod glued to the "chassis" (really just a sheet of reinforced perf board). Really helped to move the center of mass up and slow down oscillations. It's all about tuning the PID. One thing I noticed is that you have to balance the tuning of the PID just up to saturation of the servos, then pull it back a bit - seemed to be the sweet spot. Also, keep in mind that with this PID algorithm, the coefficients are proportional to the resolution AND output values of your sensor. In the end I needed to cheat with a wall power supply cuz my alkalines were just not cutting it.

Future enhancements: I'm going to see if I can squeeze in a logging function to pull data from the PID to demonstrate exactly how it controls the balance point. Tried it real time with my eb500, but no joy due to lag on servos. Thanks to all those who inspired me to experiment.

Somebody give me a problem, cuz I'm itching to experiment...

Video is here:

' BalancingBotUsingpiD.BS2
' {$PORT COM1}
' {$STAMP BS2}
' {$PBASIC 2.5}

'

---

[noparse][[/noparse] Constants ]

---

SetPoint CON 130 ' Set point
Kp CON $03A0 ' proportional constantc
Ki CON $01A0 ' integral constants
Kd CON $0100 ' derivative constants

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
ServoCenter CON 750 ' Servo not moving
IntErrorOffset CON 100
IntErrorMax CON 200
IntErrorMin CON 0


'

---

[noparse][[/noparse] Variables ]

---

pTime VAR Word
error VAR Word(4) ' Four different types of errors
pulseLeft VAR Word ' Output
pulseRight VAR Word
signA VAR Bit
signB VAR Bit
A VAR Word
B VAR Word
counter VAR Byte
p VAR Word
i VAR Word
d VAR Word

'

---

[noparse][[/noparse] Pins ]

---

Ping PIN 14
Left PIN 12
Right PIN 13
log DATA @0

'

---

[noparse][[/noparse] Initialization ]

---

DEBUG CLS
SEROUT 1,84,[noparse][[/noparse]"LABEL,TIME, Dist,P, I, D, Pulsleft, PulseRight",CR]
main:
DEBUG ?ptime
FOR counter = 0 TO 4
error(counter) = 0
NEXT
DO
LOOP UNTIL IN8=1
DO WHILE IN8=1
GOSUB Get_Ping_Distances
IF (ptime = setpoint) THEN GOTO Start
LOOP
'

---

[noparse][[/noparse] Main loop ]

---

start:
IN8=0
FREQOUT 2,2500,2000
DO
GOSUB Get_Ping_Distances
'SEROUT 1,84,[noparse][[/noparse]"DATA,TIME,",SDEC ptime,",",SDEC p,",",SDEC i,",",SDEC d,",",SDEC p+i+d,",",SDEC error,CR]
IF (pTime < setPoint - 90) OR (pTime > setPoint + 90) THEN GOTO main
GOSUB Calc_Drive
GOSUB Send_Pulse
'WRITE log+counter, ptime
'counter = counter+1
LOOP

'

---

[noparse][[/noparse] Calculate Drive ]

---

Calc_Drive:
GOSUB ErrorCalc ' Error Calcs
GOSUB PropCalc ' Perform proportional error calcs\
GOSUB IntCalc ' Perform Integral Calcs
GOSUB DerivCalc ' Perform Derivative calcs
RETURN

'

---

[noparse][[/noparse] Calculate Error ]

---

ErrorCalc:
error(Current) = SetPoint - pTime
RETURN

'

---

[noparse][[/noparse] Proportional Drive - Sign adjusted ]

---

PropCalc:
A = error(Current)
B = Kp
GOSUB Fractional_Multiply
pulseLeft = pulseLeft + A
pulseRight= pulseRight - A
'p=A
RETURN

'

---

[noparse][[/noparse] Integral Drive - Sign Adjusted ]

---

IntCalc:
error(Accumulator) = error(Accumulator) + error(Current)
error(Accumulator) = error(Accumulator) + IntErrorOffset MAX IntErrorMax MIN IntErrorMin - IntErrorOffset
A = error(Accumulator)
B = Ki
GOSUB Fractional_Multiply
pulseLeft = pulseLeft + A
pulseRight= pulseRight - A
'i=A
RETURN

'

---

[noparse][[/noparse] Derivative Drive ]

---

DerivCalc:

error(Delta) = error(Current) - error(Previous)
A = error(Delta)
B = Kd
GOSUB Fractional_Multiply
pulseLeft = pulseLeft + A
pulseRight = pulseRight - A
error(Previous) = error(Current)
'd=A
RETURN

'

---

[noparse][[/noparse] Subroutine - Get_IR_Distances ]

---

Get_Ping_Distances:
PULSOUT PING, 5
PULSIN PING, 1, pTime
IF pTime = 0 THEN GOTO Get_Ping_Distances
DEBUG HOME, CLS,?ptime
RETURN

'

---

[noparse][[/noparse] Subroutine - Send_Pulse ]

---

Send_Pulse:
PULSOUT Left, 750 + pulseRight MIN 650 MAX 850
PULSOUT Right, 749 + pulseleft MIN 649 MAX 849
pulseLeft = 0
pulseRight = 0
RETURN

'

---

[noparse][[/noparse] Subroutine - Fractional_Multiply ]

---

Fractional_Multiply:
signA = A.BIT15
signB = B.BIT15
A = ABS(A) */ ABS(B)
IF signA ^ signB THEN A = -A

RETURN

MikeS

JohnBF,
I like your Balancing Robot, great job! I have an extra BS2SX and would like to·try·to duplicate your effors. Could you please explain the purpose of the two buttons (ButtonBottom and ButtonTop). I assume that they set some "tilt" points for the PID calculation? How are they used in the balancing setup?

Thanks,
Mikes

Kas

lboucher,

I just saw the video

This is a nice design, apparently based on standard parts

Would you let us have more info on your baby, and post some more photos

If you allready did, please direct me to the right direction

Yves

lboucher said...

Finally got my bs2 controlled boe to be stable and balance on two wheels.



http://www.youtube.com/watch?v=vhJIWGOb8hg

Javalin

JohnBF

Have you still got the link to Andy's PID post - i remember seeing it but cannot find it on the search.parallax.com

good project!

Cheers,

James

Kas

here it is:

http://forums.parallax.com/showthread.php?p=529609

enjoy

yves

caskaz

Hi!.
I also want to make BalancingRobot to read this.
I had MX2125, and made motor-driver-curcuit(1 motor).
Operation for MX2125 and motor-drive are OK.
Using mG instead of ping.
mG: 30degree:+500 0degree:0 -30degree:-500

I cannot understand "PULSOUT lwheel, (1871 + drive) MIN 1625 MAX 2111" on Motors SUBROUTINES .
How do these value determine?
And right and left motor need?

quasimodo

hello
Is these someone in the "draft this week"published last week?