'{$STAMP BS2}
'{$PBASIC 2.5}

TURNON CON 1
TURNOFF CON 0
DARK CON 0
LIGHT CON 1
counter var byte
'----------------
'initialization

OUTPUT 11
initLed VAR OUT11

  'blink the LED
initLED = TURNON
PAUSE 500
initLed = TURNOFF
PAUSE 500

'beep the piezospeaker
FREQOUT 10, 500, 2637
pause 10

'----------------------------

capacitorTime var word
INPUT 1
INPUT 6
pr var in6


'main program
main:
DO

  GOSUB checkValue
  GOSUB check
LOOP





'subroutines
checkValue:
    high 6
    pause 1
    RCTIME 6,1,capacitorTime
    DEBUG ?  capacitorTime
   RETURN

check:
  IF capacitorTime > 9 THEN
    GOSUB turnleft
    GOSUB reCheck
 ELSE
    GOSUB moveForward
 ENDIF
RETURN

reCheck:
 DO UNTIL capacitorTime < 9
  GOSUB turnRight
 LOOP
RETURN

moveForward:
for counter = 1 to 2
    PULSOUT 12, 810
    PULSOUT 13, 700
    pause 20
    NEXT
   GOSUB checkValue
 RETURN

stopMove:
  pause 100
RETURN

 turnLeft:
  for counter = 1 to 4
    PULSOUT 12, 650
    PULSOUT 13, 650
    pause 20
   NEXT
  GOSUB stopMove
RETURN

turnRight:

 for counter = 1 to 2
    PULSOUT 12, 790
    PULSOUT 13, 790
    PAUSE 20
   NEXT
  GOSUB checkValue
RETURN