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

DEBUG "Program running!"

LightRight      PIN 0
LightCenter     PIN 1
LightLeft       PIN 2
LineEnable      PIN 3
LineRight       PIN 4
LineLeft        PIN 5
ObsRx           PIN 6
Stall           PIN 7
LedRight        PIN 8
LedCenter       PIN 9
LedLeft         PIN 10
Speaker         PIN 11
MotorRight      PIN 12
MotorLeft       PIN 13
ObsTxRight      PIN 14
ObsTxLeft       PIN 15

LOW MotorRight
LOW MotorLeft
PAUSE 100

counter VAR Nib
DO WHILE (counter < 4)

  PULSOUT MotorRight, 3000
  PULSOUT MotorLeft, 3000
  PAUSE 2000

  rennen VAR Bit
  rennen = Stall
    IF (rennen = 1) THEN
      PULSOUT MotorRight, 2000
      PULSOUT MotorLeft, 2000
      PAUSE 1000
      PULSOUT MotorRight, 1000
      PULSOUT MotorLeft, 1000
      PAUSE 1000
      PULSOUT MotorRight, 1500
      PULSOUT MotorLeft, 2500
      PAUSE 1000
    ENDIF

  PULSOUT MotorRight, 3000
  PULSOUT MotorLeft, 1000
  PAUSE 375

  PULSOUT MotorRight, 3000
  PULSOUT MotorLeft, 2500
  PAUSE 3000

  stopped VAR Bit
  stopped = Stall
    IF (stopped = 1) THEN
      pULSOUT MotorRight, 2000
      PULSOUT MotorLeft, 2000
      PAUSE 1000
      PULSOUT MotorRight, 1000
      PULSOUT MotorLeft, 1000
      PAUSE 1000
      PULSOUT MotorRight, 1500
      PULSOUT MotorLeft, 2500
      PAUSE 1000
    ENDIF

  eyeRight VAR Bit
  eyeLeft VAR Bit
  FREQOUT ObsTxRight, 1, 38500
    eyeRight = ObsRx
  FREQOUT ObsTxLeft, 1, 38500
   eyeLeft = ObsRx
  IF (eyeRight = 0 OR eyeLeft = 0) THEN
      PULSOUT MotorRight, 1000
      PULSOUT MotorLeft, 1000
      PAUSE 2000
      PULSOUT MotorRight, 3000
      PULSOUT MotorLeft, 1000
      PAUSE 750
    ELSE
      PULSOUT MotorRight, 3000
      PULSOUT MotorLeft, 3000
      PAUSE 2000
      stuck VAR Bit
stuck = Stall
IF (stuck = 1) THEN
    PULSOUT MotorRight, 2000
    PULSOUT MotorLeft, 2000
    PAUSE 1000
    PULSOUT MotorRight, 1000
    PULSOUT MotorLeft, 1000
    PAUSE 1000
    PULSOUT MotorRight, 1500
    PULSOUT MotorLeft, 2500
    PAUSE 1000
ENDIF
  ENDIF

quit VAR Bit
quit = Stall
IF (quit = 1) THEN
    PULSOUT MotorRight, 2000
    PULSOUT MotorLeft, 2000
    PAUSE 1000
    PULSOUT MotorRight, 1000
    PULSOUT MotorLeft, 1000
    PAUSE 1000
    PULSOUT MotorRight, 1500
    PULSOUT MotorLeft, 2500
    PAUSE 1000
ENDIF

PULSOUT MotorRight, 2000
PULSOUT MotorLeft, 2000
PAUSE 100
counter = counter + 1
LOOP
  turn VAR Nib

  DO WHILE (turn < 3)
    PULSOUT MotorRight, 3000
    PULSOUT MotorLeft, 1000
    PAUSE 750

    PULSOUT MotorRight, 1000
    PULSOUT MotorLeft, 3000
    PAUSE 750

    PULSOUT MotorRight, 2000
    PULSOUT MotorLeft, 2000
    PAUSE 100

    turn = turn + 1
  LOOP

FREQOUT Speaker, 650, 461, 466
PAUSE 100
FREQOUT Speaker, 275, 461, 466
PAUSE 100
FREQOUT Speaker, 275, 461, 466
PAUSE 100
FREQOUT Speaker, 750, 461, 466
FREQOUT Speaker, 375, 411, 416
FREQOUT Speaker, 275, 493, 498
PAUSE 100
FREQOUT Speaker, 375, 493, 498
FREQOUT Speaker, 275, 461, 466
PAUSE 100
FREQOUT Speaker, 375, 461, 466
FREQOUT Speaker, 375, 446, 451
FREQOUT Speaker, 750, 461, 466
END