'EXAMPLE PROGRAM #3
'QBASIC
'HTTP://WWW.ROBOTICS.COM/TRILOBOT

'OPEN THE SERIAL PORT.
'(MAY BE NECESSARY TO CHANGE COM PORT)
OPEN "COM1:9600,N,8,1" FOR RANDOM AS #1
 
'SETUP SCREEN
CLS
PRINT "TriloWander Program "

'HEAD STRAIGHT.
PRINT #1, "!1PH01";
R0$ = INPUT$(1, #1)

'SET EMERGENCY STOP.
PRINT #1, "!1PE01";
R0$ = INPUT$(1, #1)

WANDERLOOP:

  'READ WHISKERS
  PRINT #1, "!1GW1";
  R0$ = INPUT$(2, #1)
  GOSUB BYTETODEC

  'OBSTACLE FRONT?
  IF (R0 AND 6) <> 0 THEN
    'MOVE 12" REVERSE.
    PRINT #1, "!1PN71";
    R0$ = INPUT$(1, #1)
    'MOVE 180 DEGREES RIGHT.
    PRINT #1, "!1PN3B";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE LEFT FRONT?
  IF (R0 AND 9) = 8 THEN
    'MOVE 12" REVERSE.
    PRINT #1, "!1PN71";
    R0$ = INPUT$(1, #1)
    'MOVE 15 DEGREES RIGHT.
    PRINT #1, "!1PN33";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE RIGHT FRONT?
  IF (R0 AND 9) = 1 THEN
    'MOVE 12" REVERSE.
    PRINT #1, "!1PN71";
    R0$ = INPUT$(1, #1)
    'MOVE 15 DEGREES LEFT.
    PRINT #1, "!1PN13";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE RIGHT BACK?
  IF (R0 AND &H60) = &H40 THEN
    'MOVE 12" FORWARD.
    PRINT #1, "!1PN51";
    R0$ = INPUT$(1, #1)
    'MOVE 15 DEGREES LEFT.
    PRINT #1, "!1PN13";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE LEFT BACK?
  IF (R0 AND &H60) = &H20 THEN
    'MOVE 12" FORWARD.
    PRINT #1, "!1PN51";
    R0$ = INPUT$(1, #1)
    'MOVE 15 DEGREES RIGHT.
    PRINT #1, "!1PN33";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE BACK?
  IF (R0 AND &H60) = &H60 THEN
    'MOVE 12" FORWARD.
    PRINT #1, "!1PN51";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE LEFT SIDE?
  IF (R0 AND &H10) = &H10 THEN
    'MOVE 15 DEGREES RIGHT.
    PRINT #1, "!1PN33";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'OBSTACLE RIGHT SIDE?
  IF (R0 AND &H80) = &H80 THEN
    'MOVE 15 DEGREES LEFT.
    PRINT #1, "!1PN13";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

  'GET SONAR.
  PRINT #1, "!1GS1";
  R0$ = INPUT$(2, #1)
  GOSUB BYTETODEC
  IF R0 < 7 THEN R0 = 255 'FIX LOW VALUES.

  'IF < 12" THEN RESPOND.
  IF R0 < 12 THEN
    'MOVE 6" REVERSE.
    PRINT #1, "!1PN70";
    R0$ = INPUT$(1, #1)
    'MOVE 45 DEGREES RIGHT.
    PRINT #1, "!1PN36";
    R0$ = INPUT$(1, #1)
    GOTO WANDERGO
  END IF

WANDERGO:
  'SET WHISKER AUTO STOP TO FRONT.
  PRINT #1, "!1PW0F";
  R0$ = INPUT$(1, #1)
  'MOVE FORWARD 50' WITHOUT WAIT.
  PRINT #1, "!1PN4C";
  R0$ = INPUT$(1, #1)
  GOTO WANDERLOOP


'Add hex conversion
'subroutines here.

'Convert a hexadecimal word in R0$ to decimal in R0
WORDTODEC:
  R1$ = MID$(R0$, 1, 1)
  GOSUB NIBTODEC
  R0 = R1 * 4096
  R1$ = MID$(R0$, 2, 1)
  GOSUB NIBTODEC
  R0 = R0 + (R1 * 256)
  R1$ = MID$(R0$, 3, 1)
  GOSUB NIBTODEC
  R0 = R0 + (R1 * 16)
  R1$ = MID$(R0$, 4, 1)
  GOSUB NIBTODEC
  R0 = R0 + R1
  RETURN

'Convert a hexadecimal byte in R0$ to decimal in R0
BYTETODEC:
  R1$ = MID$(R0$, 1, 1)
  GOSUB NIBTODEC
  R0 = R1 * 16
  R1$ = MID$(R0$, 2, 1)
  GOSUB NIBTODEC
  R0 = R0 + R1
  RETURN

'Convert a hexadecimal nibble in R1$ to decimal in R1
'This routine used by BYTETODEC AND WORDTODEC.
NIBTODEC:
  IF R1$ = "A" THEN R1 = 10: RETURN
  IF R1$ = "B" THEN R1 = 11: RETURN
  IF R1$ = "C" THEN R1 = 12: RETURN
  IF R1$ = "D" THEN R1 = 13: RETURN
  IF R1$ = "E" THEN R1 = 14: RETURN
  IF R1$ = "F" THEN R1 = 15: RETURN
  R1 = VAL(R1$)
RETURN