' {$STAMP BS2}

GetSonar:

' {$PBASIC 2.5}

LOW Ping
PULSOUT Ping, 5

Ping

PIN

PingServo

15

PIN

LeftServo

PIN

RightServo

PULSIN Ping, 1, rawDist

14

13

PIN

12

IF (rawDist < 600) THEN

IF (sweepcount < 750) THEN
GOSUB Turn_Left

rawDist

VAR

sweepcount
x

VAR

VAR

pulseCount

Word
Word

Byte
VAR

ELSEIF (sweepcount > 750) THEN

GOSUB Turn_Right
ELSE

Byte

GOSUB Back_Up
ENDIF
ELSE

Main:
FOR sweepcount = 350 TO 1150 STEP 200

GOSUB Forward_Pulse
ENDIF

FOR x = 0 TO 5
PULSOUT 14, sweepcount

RETURN

PAUSE 20
NEXT
GOSUB GetSonar
NEXT

Forward_Pulse:
FOR pulsecount = 0 TO 5
PULSOUT LeftServo, 850
PULSOUT RightServo, 650

FOR sweepcount = 950 TO 550 STEP 200

FOR x = 0 TO 5
PULSOUT 14, sweepcount

PAUSE 20
NEXT
RETURN

PAUSE 20
NEXT
GOSUB GetSonar

Turn_Left:
FOR pulseCount = 0 TO 10

NEXT

PULSOUT LeftServo, 650

GOTO Main

PULSOUT RightServo, 650

PAUSE 20

NEXT

FOR pulseCount = 0 TO 3

RETURN

PULSOUT LeftServo, 750

PULSOUT RightServo, 750

Turn_Right:
FOR pulseCount = 0 TO 10

PAUSE 20
NEXT

PULSOUT LeftServo, 850

PULSOUT RightServo, 850
PAUSE 20
NEXT
RETURN

FOR pulseCount = 0 TO 7
PULSOUT LeftServo, 650
PULSOUT RightServo, 850
PAUSE 20
NEXT

Back_Up:

RETURN