' Robotics! v1.5, Program Listing 6.2: Drop-off Detection
' {$STAMP BS2} ' Stamp directive.
' {$PBASIC 2.5} ' PBASIC directive.
'----- Declarations --------------

  counter	var	nib			' For...next loop index variable.
  l_values	var	word			' Store R sensor vals for processing.
  r_values  	var 	word			' Store L sensor vals for processing.
  l_IR_freq	var	word			' Stores L IR freqs from lookup table.
  r_IR_freq	var	word			' Stores R IR freqs from lookup table.

'----- Initialization ------------

   low 13					' Initialize servo line startup values.
   low 12
   output 2					' Declare freqout lines to be outputs.
   output 7
   output 1
   freqout 2,500,3000			' Signal program is starting/restarting.

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

main:						' Main routine

  ' The command “gosub check_sensors” sends the program to a subroutine that 
  ' loads distance values into l_values and r_values.  So, when the program returns
  ' from the check_sensors subroutine, the values are updated and ready for 
  ' distance based decisions.  

  gosub check_sensors

  ' The distances are checked for four different inequalities.  Depending on the  
  ' inequality that turns out to be true, the program either branches to the
  ' forward, left_turn, right_turn or backward navigation routine. 

  if l_values >= 3 and r_values >= 3 then forward
  if l_values >= 3 and r_values < 3 then left_turn
  if l_values < 3 and r_values >= 3 then right_turn
  if l_values < 3 and r_values < 3 then backward

goto main					' Repeat the process.

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

   forward:  					' Deliver a single forward pulse, then
      pulsout 13,1000
      pulsout 12,500
      pause 10
   goto main					' go back to the main: label.

   left_turn:  				' Deliver eight left pulses, then
      for counter = 0 to 8 
         pulsout 13,500 
         pulsout 12,500 
         pause 20 
      next 
   goto main					' go back to the main: label.
      
   right_turn:  				' Deliver eight right pulses, then  
      for counter = 0 to 8 
         pulsout 13,1000 
         pulsout 12,1000 
         pause 20 
      next 
   goto main					' go back to the main: label.
   
   backward:  				' Deliver eight backward pulses, then  
      for counter = 0 to 8 
         pulsout 13,500 
         pulsout 12,1000 
         pause 20 
      next 
   goto main					' go back to the main: label.

'----- Subroutines ---------------

  ' The check sensors subroutine is a modified version of Program Listing 6.1
  ' without the Debug Terminal display.  Instead of displaying l_values and
  ' r_values, the main routine uses these values to decide which way to go.

  check_sensors:

    l_values = 0					' Reset l_values and r_values to 0.
    r_values = 0

    ' Load sensor outputs into l_values and r_values using a for...next loop,
    ' a lookup table, and bit addressing.

      for counter = 0 to 4

         check_left_sensors:
            lookup counter,[37500,38250,39500,40500,41500],l_IR_freq
            freqout 7, 1, l_IR_freq
            l_values.lowbit(counter) = ~ in8

         check_right_sensors:
            lookup counter,[37500,38250,39500,40500,41500],r_IR_freq
            freqout 1, 1, r_IR_freq
            r_values.lowbit(counter) = ~ in0

      next

    ' Convert l_values and r_values from binary to ncd format.

      l_values = ncd l_values
      r_values = ncd r_values
    
    ' Now l_values and r_values each store a number between 0 and 5 corresponding
    ' to the zone the object is detected in.  The program can now return to the 
    ' part of the main routine that makes decisions based on these distance 
    ' measurements. 

   return