'{$STAMP BS2}
'{$PBASIC 2.5}
'{$PORT COM3}

'======================================================================
'ROVERBOT_LINE_FOLLOWER.BS2
'Program:  BASIC Stamp Windows Editor Version 2.0 Beta 2.1
'Function: Follow a 0.75 inch black line on a white background
'
'======================================================================
'CONSTANTS
'General Constants
  bs2_2400       CON 396      'BS2 baud code for 2400 baud
  port_dir       CON $fd7f    'Set port bit directions in = 0, out = 1

'Motor2 Constants
  motor2_in      CON 8    'Socket 1 input line of the MicroBrain8 hub
  motor2_out     CON 9	  'Socket 1 output line of the MicroBrain8 hub

  pulse_mode     CON $88  'Enable mode (pulse_mode = right; +1 = left)

  fast           CON $3c  'Right Speed value (+ 1 = Left speed value)
  slow           CON $20
  halt           CON 0

'IREdge4 Constants
  iredge4_in     CON 10   'Socket 2 input line of the MicroBrain8 hub
  iredge4_out    CON 11	  'Socket 2 output line of the MicroBrain8 hub
  set_hi_thresh  CON $18  'Hi Thresh(sensors right to left: +0,1 2 3)
  set_lo_thresh  CON $1c  'Set Lo Thresh 
  threshold_val  CON $c0  'Threshold value(192/256 * 5 = 3.75 volts)
  comp_mask      CON $2f  'Compliment mask -> sensor outputs active low
  read_binaries  CON 4    'Get Binaries Command

'======================================================================
'VARIABLES
  sensor_data    VAR Byte   'variable holds current sensor reading
  old_data       VAR Byte   'variable holds previous sensor reading
  right_speed    VAR Byte   'motor speed
  left_speed     VAR Byte

'======================================================================
'INITIALIZATION
  DIRS = port_dir   'Stamp Directive: Set port bit directions
  old_data = 0

'Get output lines ready for serial communication
  HIGH motor2_out   
  HIGH iredge4_out
  PAUSE 50          'Wait for output lines to stabilize
	


'Set motor speed to zero to halt motors at startup
  right_speed = halt
  left_speed = halt
  GOSUB Motor_Control

'Initialize IREdge4
  SEROUT iredge4_out, bs2_2400, [comp_mask]
  PAUSE 2000	'Wait 2 seconds to get hands free of robot

'======================================================================
'MAIN PROGRAM
'Read the four sensors and depending on their respective values, send 'commands to the motors to differentially steer the robot along the 'black line

DO
  'Read the QRB1134 sensor bit data
  SEROUT iredge4_out, bs2_2400, [read_binaries]
  SERIN iredge4_in, bs2_2400, [sensor_data]
  IF sensor_data = old_data THEN Done  ‘skip motor setting if no change

  'Else, use sensor data to differentially steer along the line
  SELECT sensor_data
    CASE 0
      'Lost the line completely – stop before a tragedy occurs!
      right_speed = halt
      left_speed = halt + 1
      
    CASE 1
      'Losing line rapidly to the left; slow down and make right turn
      right_speed = halt
      left_speed = fast +1
      
    CASE 3
      'Starting to loose line to the left make a right turn
      right_speed = halt
      left_speed = slow + 1
      
    CASE 6
      'Tracking line continue forward
      right_speed = fast
      left_speed =  fast + 1
      
    CASE 8
      'Losing line rapidly to the right; make Left turn
      right_speed = fast
      left_speed =  halt + 1
      
    CASE 12
      'Starting to lose line make a left turn
      right_speed = slow
      left_speed = halt + 1

    CASE 2,4,5,7,9,10,11,13,14,15
      'Shouldn't happen so must be an erroneous sensor reading
      GOTO Done
  ENDSELECT
  GOSUB Motor_Control
  old_data = sensor_data
  Done:
      PAUSE 1	'Do nothing – get new sensor reading
LOOP

'======================================================================
'SUBROUTINES

Motor_Control:
  'Send speed commands to right and left motors
  SEROUT motor2_out, bs2_2400, [right_speed]
  SEROUT motor2_out, bs2_2400, [left_speed]
RETURN