Ping Ultrasonic Sensor "Maze"

[sCRYed]

Hey guy, I am kind of new to this and i am looking for any help i could get. I have a Ping Ultrasonic Sensor and i am trying to get it to solve any maze it is put into. Unfortunately i am at a loss. I have managed to get it so sense when it was close to a wall and move away from it, but it can't seem to always turn the right way. This is the code i am using....

' {$STAMP BS2}
' {$PBASIC 2.5}
Ping PIN 15
pulseCount VAR Byte

#SELECT $stamp
#CASE BS2, BS2E
Trigger CON 5
Scale CON $200
#CASE bs3sx, BS2P, BS2PX
Trigger CON 13
Scale CON $0CD
#CASE BS2PE
Trigger CON 5
Scale CON $1E1
#ENDSELECT
RawToIn CON 889
RawToCm CON 2257

IsHigh CON 1
IsLow CON 0


rawDist VAR Word
inches VAR Word
cm VAR Word


Rest:
DEBUG CLS,
"Parallax ping))) sonar", CR,
"======================", CR,
CR,
"time (uS)..... ", CR,
"inches........ ", CR,
"centermeters.. "


Main:

DO

GOSUB Get_Sonar
inches = rawDist ** RawToIn
cm = rawDist ** RawToCm

DEBUG CRSRXY, 15, 3,
DEC rawDist, CLREOL,
CRSRXY, 15, 4,
DEC inches, CLREOL,
CRSRXY, 15, 5,
DEC cm, CLREOL
IF cm < 6 AND cm > 0 THEN
GOSUB Turn_Left
ELSEIF cm > 6 AND cm < 8 THEN
GOSUB Turn_Right
ELSE
GOSUB Forward_Pulse
ENDIF

LOOP
END





Get_Sonar:
Ping = IsLow
PULSOUT Ping, Trigger
PULSIN Ping, IsHigh, rawDist
rawDist = rawDist */ Scale
rawDist = rawDist / 2
RETURN

Forward_Pulse:
PULSOUT 13, 850
PULSOUT 12, 650
RETURN

Turn_Left:
FOR pulseCount = 0 TO 40
PULSOUT 13, 650
PULSOUT 12, 650
NEXT
RETURN

Turn_Right:
FOR pulseCount = 0 TO 40
PULSOUT 13, 850
PULSOUT 12, 850
NEXT
RETURN

Back_Up:
FOR pulseCount = 0 TO 80
PULSOUT 13, 650
PULSOUT 12, 850
NEXT
RETURN

Does anyone know what to do? I would accept any help. Thanks.