Yet another balancing robot

Kas

As you may see, the hammer is the only non Parallax hi Tech component in this project
it does raise the center of gravity and is very convenient for grasping the unit in case
of emergency

Tips:
Use a BS2P (plain BS2 not fast enough)
A fifth NiMh battery allows to get a real 6V to power the servos
Total measuring loop is adjusted to 20 ms using a scope
The two buttons allow balance fine tuning
The two LEDs give a clear indication of what is going on

A YouTube video: http://www.youtube.com/v/9rcecbdKbuY
**edit: I still have to find a better way to include a video...

Ping))) caveat:
The ping signal is not a monotonous function; the same pTime value may correspond to
slihtly different distances/angles. A sweet point has to be determined by moving the
sensor to adjust the distance with the ground·

Here is the PBasic code:

//

---

' {$STAMP BS2p}
' {$PBASIC 2.5}

' Balancing robot with Ping))) sensor
' kas, May 2008
' Original: John Fisher, January 2007
' Setpoint original = 525
'···· - ** -············ Multiply by 0.XXXX
'To find the ** constant, multiply the fraction part by 65536.
'For 0.72562: the ** constant would be 0.72562 * 65536 = 47554
'···· - */ -············ Multiply by XX.XXXX (max 255.996)
'To find the */ constant, multiply the value by 256 and convert to integer.
'For Pi: The */ constant would be INT(3.14159 * 256) = 804 ($0324)

'

---

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

---

· buttonBottom······ PIN····· 1
· buttonTop··········· PIN·····5
· LedAv················· PIN···· 3
· LedAr··················PIN···· 7
· rwheel················ PIN···· 12
· lwheel················ PIN···· 13
· ping··················· PIN···· 14
· speaker·············· PIN···· 15

'

---

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

---

· Scale················· CON····· $0CD··· ' raw x 0.80 = uS (with */)
· RawToCm··········· CON····· 2257··· ' 1 / 29.034 (with **)
· RawToMm··········· CON····· 22572·· ' 1 / 2.9034 (with **: 0.34442 * 65536)
· Kp····················· CON····· 2······ ' PID constants
· Ki······················ CON····· 3
· Kd····················· CON····· 4
· Current···············CON····· ·0······ ' error array index
· 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
·· mm················ VAR···· Word·········· ' distance
·· counter··········· VAR···· Nib

'

---

[noparse][[/noparse] INITIALIZATION ]

---

· '#DEFINE DISPLAYDATA
· '#DEFINE CALIB······························· '
Reset:
· GOSUB ProgramStartSound
· #IF CALIB #THEN
· PAUSE 2000
· PULSOUT ping, 12
· PULSIN ping, 1, pTime
· debub CRSRXY, 0, 2, "pTime initial..", DEC4 pTime,CLREOL
· SetPoint = pTime
· #ELSE
· SetPoint = 1055
· #ENDIF
· #IF DISPLAYDATA #THEN
··· DEBUG CLS,
········· "Balancing Boe Bot", CR,············ ' setup report screen
········· "Kp:", DEC2 Kp, "· Kd:", DEC2 Kd, "· Ki:", DEC2 Ki, CR, CR,
········· "pTime ........ ", CR,
········· "Distance...... ", CR,
········· "Drive......... ", CR
· #ENDIF

'

---

[noparse][[/noparse]PROGRAM CODE ]

---

Main:
· GOSUB CheckButtons
· DO
··· GOSUB GetPingData
··· IF (pTime<(SetPoint-100))OR(pTime>(SetPoint+100))THEN GOTO Main·· 'Shut down motors if beyond recoverable tilt angle
··· GOSUB ComputeError
··· GOSUB SetMotors
··· GOSUB SetLEDs
··· #IF DISPLAYDATA #THEN
··· GOSUB DisplayInfo
··· #ELSE
··· PAUSE 12································· ' To be adjusted (20 ms)
··· #ENDIF
· LOOP
· END

'

---

[noparse][[/noparse] SUBROUTINES ]

---

GetPingData:
· PULSOUT ping, 12
· PULSIN ping, 1, pTime
RETURN

ComputeError:
· 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
· error(SecondtoLast) = error(Last)
· error(Last) = error(Current)
RETURN

SetMotors:
· PULSOUT lwheel, (1875 -8 + drive) MIN 1625 MAX (2125-8)
· PULSOUT rWheel, (1875 - drive) MIN 1625 MAX 2125
RETURN

SetLEDs:
· IF setPoint > pTime THEN
··· HIGH LedAv : LOW LedAr
· ELSEIF setPoint = pTime THEN
··· HIGH LedAv : HIGH LedAr
· ELSE
··· LOW LedAv : HIGH LedAr
· ENDIF
RETURN

CheckButtons:
· LOW LedAv : LOW LedAr
· IF buttonTop = 0 THEN························· 'Ajuste SetPoint
··· SetPoint = SetPoint + 1
··· FREQOUT speaker, 12, 1350··················· ' confirm button push
··· DEBUG CRSRXY, 15, 3, DEC4 SetPoint
··· PAUSE 200
· ENDIF
· IF buttonBottom = 0 THEN
··· SetPoint = SetPoint - 1
··· FREQOUT speaker, 12, 1350··················· ' confirm button push
··· DEBUG CRSRXY, 15, 3, DEC4 SetPoint
··· PAUSE 200
· ENDIF
RETURN

DisplayInfo:
' At sea level sound travels through air at 1130 feet per second.
' This is 1 cm in 29.034 uS. Result is /2 for return of echo.
· mm = ((pTime */ Scale)/2) ** RawToMm·························· ' convert to centimeters
· DEBUG CRSRXY, 15, 3, DEC4 pTime, CLREOL,
······· CRSRXY, 15, 4, DEC4 mm, CLREOL,
······· CRSRXY, 15, 5, DEC4 drive, CLREOL
RETURN

ProgramStartSound:
· FOR counter = 4 TO 1
··· FREQOUT speaker, 30, (1320) - (counter * 36)
··· PAUSE 100
· NEXT
RETURN

---

//

Thanks to Tronic and JohnBF for transmitting me the desire to undertake this project
and Andy Lindsay for his very professional introduction to PID control
Next step: same project, replacing Ping))) by a Memsic 2125 accelerometer


Post Edited (Kas) : 6/1/2008 6:32:15 AM GMT

FireHopper

NEAT!

bobledoux

You converted a Boebot into a balancing robot. That has possibilities for Parallax. I see a new kit on the horizon.

jslil

I have been wanting to see this completed with an inclinometer as opposed to a vision system. How unstable was the system without the hammer?

Kas

jslil said...
I have been wanting to see this completed with an inclinometer as opposed to a vision system. How unstable was the system without the hammer?

I removed the hammer again and tried it for you
The system will balance for less than 5 seconds, with a lot of movement from the motors
With the hammer back, it is stable for several minutes

LSB

bobledoux said...
You converted a Boebot into a balancing robot. That has possibilities for Parallax. I see a new kit on the horizon.

Yeah--- the Boe Vila.

bryan mcdonald

beauty!