' Robotics! v1.5, Program Listing 5.3:  IR Roaming by Numbers in Real Time
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
'----- Declarations --------------
 							' The lower 2 bits of the 
  sensors var nib					' sensors variable are used to store
 							' IR detector values.
'----- Initialization ------------

  output 2						' Set all I/O lines sending freqout
  output 7						' signals to function as outputs
  output 1
  freqout 2, 2000, 3000				' Program start/restart signal.
  low 12						' Set P12 and 13 to output-low.
  low 13

'----- Main Routine --------------

main:
							' Detect object on the left.
   freqout 7,1,38500				' Send freqout signal - left IRLED.
   sensors.bit0 = in8				' Store IR detector output in RAM.
							' Detect object on the right.
   freqout 1,1,38500				' Repeat for the right IR pair.
   sensors.bit1 = in0

   pause 18						' 18 ms pause(2 ms lost on freqout).

   ' By loading the IR detector output values into the lower 2 bits of the sensors
   ' variable, a number btwn 0 and 3 that the branch command can use is generated.

   branch sensors,[backward,left_turn,right_turn,forward]

'----- Navigation Routines -------

      forward:     pulsout 13,1000: pulsout 12,500: goto main
      left_turn:   pulsout 13,500: pulsout 12,500: goto main
      right_turn:  pulsout 13,1000: pulsout 12,1000: goto main
      backward:    pulsout 13,500: pulsout 12,1000: goto main