' {$STAMP BS2}
' {$PBASIC 2.5}
' LineFollowWithCheckQtis.bs2
' Navigates based on values acquired with the
' Ceck_Qtis subroutine.


qtis VAR Nib                ' black/white states
acoustic VAR Nib
OUTB = %1111                ' Set OUTB bits to 1


DO                          ' Main DO...LOOP
IF (IN14 = 0) AND (IN15 = 0) THEN
    GOSUB Acoustic_Turn
    GOSUB Blink_LED
  ELSEIF (IN14 = 0) THEN
    GOSUB Acoustic_Turn
    GOSUB Blink_LED
  ELSEIF (IN15 = 0) THEN
    GOSUB Acoustic_Turn
    GOSUB Blink_LED
  ELSE
    GOSUB Check_Qtis
ENDIF

  GOSUB Check_Qtis          ' Get QTI states

  SELECT qtis               ' Control servo
                            ' speeds/directions
    CASE %1000              ' Rotate right
      PULSOUT 13, 650
      PULSOUT 12, 650
    CASE %1100              ' Pivot right
      PULSOUT 13, 750
      PULSOUT 12, 650
    CASE %0100              ' Curve right
      PULSOUT 13, 800
      PULSOUT 12, 650
    CASE %0110              ' Straight ahead
      PULSOUT 13, 850
      PULSOUT 12, 650
    CASE %0010              ' Curve left
      PULSOUT 13, 850
      PULSOUT 12, 700
    CASE %0011              ' Pivot left
      PULSOUT 13, 850
      PULSOUT 12, 750
    CASE %0001              ' Rotate left
      PULSOUT 13, 850
      PULSOUT 12, 850
    CASE %0000
      PULSOUT 13, 800
      PULSOUT 12, 800
      PAUSE 40
      PULSOUT 13, 600
      PULSOUT 12, 600
      PAUSE 40
    CASE ELSE               ' Do nothing
      PAUSE 3
  ENDSELECT

LOOP

Check_Qtis:

' Result -> qtis variable.
' 0 means white surface
' 1 means black surface.

  DIRB = %1111              ' P7..P4 -> output
  PAUSE 0                   ' Delay = 230 us
  DIRB = %0000              ' P7..P4 -> input
  PAUSE 0                   ' Delay = 230 us
  qtis = INB                ' Store QTI output
                            ' in INB
  RETURN

Acoustic_Turn:

FOR acoustic = 1 TO 80
  PULSOUT 13, 650
  PULSOUT 12, 650
  PAUSE 20
NEXT
  END
RETURN
Blink_LED:

HIGH 0
PAUSE 500
LOW 0
PAUSE 500
HIGH 0
PAUSE 500
LOW 0
PAUSE 500
HIGH 0
PAUSE 500
LOW 0
PAUSE 500
HIGH 0
PAUSE 500
LOW 0
PAUSE 500
HIGH 0
PAUSE 500
LOW 0
PAUSE 500
END
RETURN