Line Following

Explanation
This program uses the O.A.R. Sensor Board to follow a black line. It takes the information given by the two sensors on the front to track it. Whenever a sensor is reading white the robot reacts. If the left sensor is reading white then robot needs to turn right to recover it. If the right sensor is reading white then we need to turn left. If for any reason both sensors are reading white, in this case, the robot will back up to try to recover it.

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

'Constants definitions
BLACK  CON  0
WHITE  CON  1
RWHEEL CON  12
'650 is forward, 750 stop, 850 back

LWHEEL CON  13
'850 is forward, 750 stop, 650 back

'Variable definitions
'sl VAR Bit  'right side sensor
'sr VAR Bit  'left side sensor
fr VAR Bit  'right front sensor
fl VAR Bit  'left front sensor

DO
  fr = IN4   'yellow
  'sl = IN5   'blue
  'sr = IN6   'white
  fl = IN7   'green

  ' 0 is black
  ' 1 is white

  'DEBUG CLS
  'DEBUG "fr = ",DEC fr, CR
  'DEBUG "fl = ",DEC fl, CR
  'DEBUG "sr = ",DEC sr, CR
  'DEBUG "sl = ",DEC sl, CR

'normal navigation
     IF (fr = BLACK) AND (fl = BLACK) THEN 'move forward
        'DEBUG "Moving forward"
        PULSOUT RWHEEL, 720
        PULSOUT LWHEEL, 780
        'PAUSE 1
     ELSEIF (fr = WHITE) AND (fl = BLACK) THEN   'right out turn left
        PULSOUT RWHEEL, 720
        PULSOUT LWHEEL, 750
       ' PAUSE 1
    ELSEIF (fr = BLACK) AND (fl = WHITE) THEN    'left out turn right
        PULSOUT RWHEEL, 750
        PULSOUT LWHEEL, 780
        'PAUSE 1
    ELSE                               'back up
        PULSOUT RWHEEL, 800
        PULSOUT LWHEEL, 700
        'PAUSE 1
    ENDIF

  'Turn on LED if we're moving forward
  IF (sr = BLACK) OR (sl = BLACK) THEN
      HIGH 9
  ELSE
      LOW 9
  ENDIF

  PAUSE 20

LOOP