Campus Nav

Explanation
Program to navigate a map made out of black lines. The difference with the line following program is that in this case the robot can "see" crossings and decide where to go. In this program the robot uses all of the sensors that the sensors board has. The decision of where to turn is left to chance in this case.

' {$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

reps VAR Word

tog VAR Nib
tog = 1

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

  ' 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

     IF (sl = WHITE) AND (fl = BLACK) AND (fr = BLACK) AND (sr = WHITE) THEN 'move forward
        'DEBUG "Moving forward"
        PULSOUT RWHEEL, 720
        PULSOUT LWHEEL, 780
        'PAUSE 1
     ELSEIF (fl = BLACK) AND (fr = WHITE) AND (sr = WHITE) THEN   'right out turn left
        PULSOUT RWHEEL, 720
        PULSOUT LWHEEL, 750
       ' PAUSE 1
    ELSEIF (sl = WHITE) AND (fl = WHITE) AND (fr = BLACK) THEN    'left out turn right
        PULSOUT RWHEEL, 750
        PULSOUT LWHEEL, 780
        'PAUSE 1

    ELSEIF((fr = BLACK) AND (fl = BLACK)) AND ((sr = BLACK) OR (sl = BLACK)) THEN

        GOSUB Intersection
        'PULSOUT RWHEEL, 800
        'PULSOUT LWHEEL, 700

    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

Intersection:

    DEBUG CLS, "INTERSECTION"

    'RIGHT TURN
    IF (tog = 0)  THEN
      tog = 1
      'turn right 90deg
      FOR reps = 1 TO 120

        PULSOUT LWHEEL, 780
        PULSOUT RWHEEL, 780
        PAUSE 1
      NEXT

      FOR reps = 1 TO 40
        'move forward a little
        PULSOUT LWHEEL, 780
        PULSOUT RWHEEL, 720
        PAUSE 1
      NEXT
    RETURN

    'LEFT TURN
    ELSEIF (tog = 1)  THEN
      tog = 2
      'turn left 90deg
      FOR reps = 1 TO 120

        PULSOUT LWHEEL, 720
        PULSOUT RWHEEL, 720
        PAUSE 1
      NEXT

      FOR reps = 1 TO 40
        'move forward a little
        PULSOUT LWHEEL, 780
        PULSOUT RWHEEL, 720
        PAUSE 1
      NEXT
    RETURN

    'go straight
    ELSEIF (tog = 2)  THEN
      tog = 0
      FOR reps = 1 TO 60
        'move forward a little
        PULSOUT LWHEEL, 780
        PULSOUT RWHEEL, 720
        PAUSE 1
      NEXT
    RETURN

    ELSE
    RETURN
    ENDIF