Assistance with Robot Programming

[dean93]

I'm trying to program a robot to scan an area with its sensors find an available parking space and go park, then reverse out of the space and back into it, all while surrounded surrounded by other robots in spaces and not touching any of its surroundings. My professor really through me into the water with this project and I'm not much of a programmer so any help is appreciated.

[' {$STAMP BS2}
' {$PBASIC 2.5}
______________________________________________________________________
VARIABLES & CONSTANTS

pulseCount VAR Byte ' For...next loop counter.
counter VAR Nib ' Counts alternate contacts.
Frequency Select VAR Nib
irFrequency VAR Word
irDetectLeft VAR Bit
irDetectRight VAR Bit
Distance Left VAR Nib
Distance Right VAR Nib
______________________________________________________________________
INITIATION

DEBUG CLS,
"IR OBJECT ZONE", CR,
"Left Right", CR,
"----- -----"

FREQOUT 4, 2000, 3000 ' Signal program start/reset.

MAIN ROUTINE

DO

GOSUB Get Distances
GOSUB Display Distances
GOSUB FORWARD
GOSUB Stop Quickly
GOSUB BACKWRDS
GOSUB LEFT
GOSUB BACKWRDS
GOSUB Stop Quickly
GOSUB FORWARD
GOSUB Stop Quickly
GOSUB RIGHT

LOOP
_______________________________________________________________________
SUBROUTINES


FORWARD:
FOR counter = 1 TO 64 ' FORWARD
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 200
NEXT
RETURN




BACKWARDS:
FOR counter = 1 TO 64 ' BACKWARDS
PULSOUT 13, 650
PULSOUT 12, 850
PAUSE 200
NEXT
RETURN


LEFT:
FOR counter = 1 TO 24 ' LEFT
PULSOUT 13, 650
PULSOUT 12, 650
PAUSE 200
NEXT
RETURN


RIGHT:
FOR counter = 1 TO 24 ' RIGHT
PULSOUT 13, 850
PULSOUT 12, 850
PAUSE 200
NEXT
RETURN

Stop Quickly:
' This stops the wheels so the Boe-Bot does not "coast" forward.
PULSOUT 13, 750
PULSOUT 12, 750
PAUSE 20
RETURN


Get Distances:
Distance Left = 0
Distance Right = 0
FOR Frequency Select = 0 TO 4
LOOKUP Frequency Select,[37500,38250,39500,40500,41500], irFrequency
FREQOUT 8,1,irFrequency
irDetectLeft = IN9
Distance Left = Distance Left + irDetectLeft
FREQOUT 2,1,irFrequency
irDetectRight = IN0
Distance Right = Distance Right + irDetectRight
PAUSE 100
NEXT
RETURN


Display Distances:
DEBUG CRSRXY,2,3, DEC1 Distance Left,
CRSRXY,9,3, DEC1 Distance Right
RETURN


END




]