' -----[ Title ]--------------------------------------------------------------
' Robotics with the Boe-Bot - DisplayBothDistances.bs2
' Test IR detector distance responses of both IR object detectors to
' frequency sweep.

' {$STAMP BS2}                               ' Stamp directive.
' {$PBASIC 2.5}                              ' PBASIC directive.

' -----[ Variables ]----------------------------------------------------------

freqSelect     VAR     Nib
irFrequency    VAR     Word
irDetectLeft   VAR     Bit
irDetectRight  VAR     Bit
distanceLeft   VAR     Nib
distanceRight  VAR     Nib

' -----[ Initialization ]-----------------------------------------------------

DEBUG CLS,
      "IR OBJECT ZONE", CR,
      "Left   Right", CR,
      "-----  -----"

' -----[ Main Routine ]-------------------------------------------------------

DO

  GOSUB Get_Distances
  GOSUB Display_Distances

LOOP

' -----[ Subroutine – Get_Distances ]-----------------------------------------

Get_Distances:

  distanceLeft = 0
  distanceRight = 0

  FOR freqSelect = 0 TO 4

    LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency

    FREQOUT 8,1,irFrequency
    irDetectLeft = IN9
    distanceLeft = distanceLeft + irDetectLeft

    FREQOUT 2,1,irFrequency
    irDetectRight = IN0
    distanceRight = distanceRight + irDetectRight

    PAUSE 100

  NEXT

  RETURN

' -----[ Subroutine – Display_Distances ]-------------------------------------

Display_Distances:

  DEBUG CRSRXY,2,3, DEC1 distanceLeft,
        CRSRXY,9,3, DEC1 distanceRight
  RETURN