' =========================================================================
'
'   File....... Mailbot.BS2
'   Purpose....
'   Author.....
'   E-mail.....
'   Started....
'   Updated....
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

ServoL          PIN     13                      ' left servo
ServoR          PIN     12                      ' right servo
Sensor          PIN     3                       ' input from QTI sensors


' -----[ Constants ]-------------------------------------------------------

Active          CON     1
NotActive       CON     0


' -----[ Variables ]-------------------------------------------------------

prox            VAR     Bit                     ' proximity flag
idx             VAR     Nib                     ' loop control
qti             VAR     Nib                     ' sensor array status


' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Reset:
  LOW ServoL                                    ' initialize servo pins
  LOW ServoR


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Check_Prox                            ' check proximity
    IF (prox = Active) THEN

      ' deal with proximity sensor

    ENDIF

    GOSUB Scan_QTIs:                            ' scan line array

    ' move bot

    SELECT qti
      CASE %0010, %1010                         ' Forward
        PULSOUT ServoL, 850
        PULSOUT ServoR, 650

     CASE %1011, %0011                          ' Pivot right
        PULSOUT ServoL, 850
        PULSOUT ServoR, 750

      CASE %1001, %0001                         ' Rotate right
        PULSOUT ServoL, 850
        PULSOUT ServoR, 850

      CASE %1110, %0110                         ' Pivot Left
        PULSOUT ServoL, 750
        PULSOUT ServoR, 650

      CASE %1100, %0100                         ' Rotate Left
        PULSOUT ServoL, 650
        PULSOUT ServoR, 650

      CASE %1111                                ' Cross Detection
        PULSOUT ServoL, 850                     ' -- Continue foward
        PULSOUT ServoR, 650

    ENDSELECT

    PAUSE 20
  LOOP


' -----[ Subroutines ]-----------------------------------------------------

Check_Prox:

  ' code to check proximity sensor

  RETURN



' Activates and reads QTI sensors on pins 5 - 8

Scan_QTIs:
  FOR idx = 0 TO 3                              ' loop through 4 bits
    HIGH (5 + idx)                              ' activate sensor
    PAUSE 1
    qti.LOWBIT(idx) = Sensor                    ' record sensor output
  NEXT
  RETURN 