'--------------------------------------------------------------------
'TRILOBOT PROGRAM
'

START:

    'SETUP COMMUNICATIONS PORT ERROR HANDLER.
    ON ERROR GOTO IOERROR
   
    GOSUB DISPLAY       'DISPLAY SCREEN.
    GOSUB GETPORT       'ASK FOR COMM PORT.
    GOSUB OPENPORT      'OPEN COMM PORT.

    'BEEP TWICE.
    CMND$ = "!1PS02": COUNT = 1: TIMEOUT = 10: GOSUB SEND

    'GET ROBOT STATUS INFORMATION.
    GOSUB GETSTATUS
   
MENULOOP:

    'GET ENTRY.
    GOSUB CLEARINPUT
    COLOR 7, 1
    LOCATE 21, 4
    PRINT "Enter Selection: ";
    DO: ENTRY$ = INKEY$: LOOP UNTIL ENTRY$ <> ""
    PRINT ENTRY$;

    'MANUALLY ENTER COMMANDS.
    IF UCASE$(ENTRY$) = "M" THEN
        COLOR 7, 1
        LOCATE 21, 4
        INPUT "Enter chacters to send ", CMND$
        LOCATE 22, 4
        INPUT "Enter # of response characters ", COUNT
        LOCATE 23, 4
        INPUT "Enter timeout in seconds ", TIMEOUT
        GOSUB SEND
        LOCATE 24, 4
        PRINT "Response: "; RESPONSE$; "   Press any key";
        DO: LOOP UNTIL INKEY$ <> ""
    END IF
   
   
    'INPUTS.
    IF UCASE$(ENTRY$) = "I" THEN
        COLOR 7, 1
        CMND$ = "!1GU1": COUNT = 2: TIMEOUT = 10: GOSUB SEND
        LOCATE 22, 4: PRINT "Digital Input Port: "; RESPONSE$;
        CMND$ = "!1GU2": COUNT = 2: TIMEOUT = 10: GOSUB SEND
        LOCATE 23, 4: PRINT "Analog Input Port: "; RESPONSE$;
        LOCATE 24, 4
        PRINT "Press any key";
        DO: LOOP UNTIL INKEY$ <> ""
    END IF
       

    'OUTPUTS.
    IF UCASE$(ENTRY$) = "O" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "1=on, 0=off "; HUH$
        HUH$ = UCASE$(HUH$)
        IF V$ = "1" THEN CMND$ = "!1PP01"
        IF V$ = "0" THEN CMND$ = "!1PP00"
        COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF
       

    'SERVO CONTROL.
    IF UCASE$(ENTRY$) = "V" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "Enter servo # (1-8) "; RCS
        LOCATE 23, 4
        INPUT "Enter desired servo position (1-250) "; RCP
        RCS$ = "0" + HEX$(RCS): RCS$ = RIGHT$(RCS$, 2)
        RCP$ = "0" + HEX$(RCP): RCP$ = RIGHT$(RCP$, 2)
        CMND$ = "!1PR" + RCS$ + RCP$: COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF

   
    'SOUND EFFECTS.
    IF UCASE$(ENTRY$) = "S" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "1=Beep, 2=hello "; HUH$
        IF HUH$ = "1" THEN CMND$ = "!1PS01"
        IF HUH$ = "2" THEN CMND$ = "!1PS82"
        COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF


    'GRIPPER MOTION.
    IF UCASE$(ENTRY$) = "G" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "1=gripper down/open, 2=close/up "; HUH$
        IF HUH$ = "1" THEN CMND$ = "!1PG101"
        IF HUH$ = "2" THEN CMND$ = "!1PG102"
        COUNT = 1: TIMEOUT = 5: GOSUB SEND
    END IF

   
    'HEAD MOTION.
    IF UCASE$(ENTRY$) = "H" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "1=head straight, 2=up, 3=down, 4=left, 5=right: "; HUH$
        IF HUH$ = "1" THEN CMND$ = "!1PH101"
        IF HUH$ = "2" THEN CMND$ = "!1PH102"
        IF HUH$ = "3" THEN CMND$ = "!1PH103"
        IF HUH$ = "4" THEN CMND$ = "!1PH105"
        IF HUH$ = "5" THEN CMND$ = "!1PH107"
        COUNT = 1: TIMEOUT = 5: GOSUB SEND
    END IF


    'ROTATE.
    IF UCASE$(ENTRY$) = "R" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "L=left, R=right: "; HUH$
        HUH$ = UCASE$(HUH$)
        IF HUH$ = "L" THEN CMND$ = "!1PN19"
        IF HUH$ = "R" THEN CMND$ = "!1PN39"
        COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF
   
   
    'LIGHTS.
    IF UCASE$(ENTRY$) = "L" THEN
        COLOR 7, 1
        LOCATE 22, 4
        INPUT "0=All off, 1=All lights on: "; HUH$
        IF HUH$ = "0" THEN CMND$ = "!1PL00"
        IF HUH$ = "1" THEN CMND$ = "!1PL01"
        COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF
   
   
    'UPDATE CONTINOUSLY.
    IF UCASE$(ENTRY$) = "C" THEN
        LOCATE 23, 4
        COLOR 7, 1
        PRINT "Updating status continously, press any key to stop . . .";
        DO
            GOSUB GETSTATUS
        LOOP WHILE INKEY$ = ""
    END IF
      

    'UPDATE.
    IF UCASE$(ENTRY$) = "U" THEN
        GOSUB GETSTATUS
    END IF
      

    'TEST PROGRAM.
    IF UCASE$(ENTRY$) = "T" THEN
      'put a test program here.
    END IF
       

    'DRIVE.
    IF UCASE$(ENTRY$) = "D" THEN
        COLOR 7, 1
        LOCATE 21, 4
        INPUT "F=Forward, R=Reverse ", HUH$
        HUH$ = UCASE$(HUH$)
        IF HUH$ = "F" THEN CMND$ = "!1PN41"
        IF HUH$ = "R" THEN CMND$ = "!1PN61"
        COUNT = 1: TIMEOUT = 10: GOSUB SEND
    END IF
      

    'QUIT.
    IF UCASE$(ENTRY$) = "Q" THEN
        CLS
        END
    END IF

   
    'REFRESH SCREEN.
    IF ENTRY$ = CHR$(13) THEN
        GOSUB DISPLAY       'DISPLAY SCREEN.
        GOSUB GETSTATUS
    END IF

    GOTO MENULOOP


'--------------------------------------------------------------------
'ERROR HANDLER.
'
'SIMPLY RESUMES WHEN ERROR OCCURS.
'
IOERROR:
        RESUME


'--------------------------------------------------------------------
'DISPLAY A DOUBLE-LINE BOX.
'
'RECEIVES: BOXSTARTX, BOXSTARTY, BOXSIZEX , BOXSIZEY
'
BOX:
    'DRAW TOP LINE
    LOCATE BOXSTARTY, BOXSTARTX
    PRINT CHR$(201);
    FOR X = 1 TO BOXSIZEX - 2: PRINT CHR$(205); : NEXT X
    PRINT CHR$(187);
      
    'DRAW MIDDLE LINES
    FOR Y = 1 TO BOXSIZEY - 2
        LOCATE BOXSTARTY + Y, BOXSTARTX
        PRINT CHR$(186);
        LOCATE BOXSTARTY + Y, BOXSTARTX + BOXSIZEX - 1
        PRINT CHR$(186);
    NEXT Y
      
    'DRAW BOTTOM LINE
    LOCATE BOXSTARTY + BOXSIZEY - 1, BOXSTARTX
    PRINT CHR$(200);
    FOR I = 1 TO BOXSIZEX - 2: PRINT CHR$(205); : NEXT I
    PRINT CHR$(188);

    RETURN


'--------------------------------------------------------------------
'DISPLAYS ENTIRE SCREEN WITH MENU.
'
DISPLAY:

    'HEADING.
    COLOR 7, 1: CLS
    LOCATE 1, 1: COLOR 0, 7: PRINT SPACE$(80);
    LOCATE 1, 28: PRINT "Trilobot Control Program";
    LOCATE 1, 75: PRINT "V2.0";
    COLOR 7, 1

    'BOXES.
    BOXSTARTX = 1: BOXSTARTY = 2: BOXSIZEX = 80: BOXSIZEY = 3: GOSUB BOX
    BOXSTARTX = 1: BOXSTARTY = 5: BOXSIZEX = 50: BOXSIZEY = 15: GOSUB BOX
    BOXSTARTX = 51: BOXSTARTY = 5: BOXSIZEX = 30: BOXSIZEY = 15: GOSUB BOX
    BOXSTARTX = 1: BOXSTARTY = 20: BOXSIZEX = 80: BOXSIZEY = 6: GOSUB BOX
     
    'BOX TITLES.
    COLOR 15, 1: LOCATE 5, 20: PRINT " Commands ";
    COLOR 15, 1: LOCATE 5, 62: PRINT " Status ";
                       
    'COMMANDS.
    LOCATE 6, 4: COLOR 14, 1: PRINT "S";
    COLOR 7, 1: PRINT " - Sound Effects";
   
    LOCATE 7, 4: COLOR 14, 1: PRINT "G";
    COLOR 7, 1: PRINT " - Gripper Motion";
  
    LOCATE 8, 4: COLOR 14, 1: PRINT "H";
    COLOR 7, 1: PRINT " - Head Motion";
 
    LOCATE 9, 4: COLOR 14, 1: PRINT "R";
    COLOR 7, 1: PRINT " - Rotate";

    LOCATE 10, 4: COLOR 14, 1: PRINT "M";
    COLOR 7, 1: PRINT " - Manual Entry";
  
    LOCATE 11, 4: COLOR 14, 1: PRINT "L";
    COLOR 7, 1: PRINT " - Lights";
  
    LOCATE 12, 4: COLOR 14, 1: PRINT "D";
    COLOR 7, 1: PRINT " - Drive";
  
    LOCATE 13, 4: COLOR 14, 1: PRINT "U";
    COLOR 7, 1: PRINT " - Update Status";
  
    LOCATE 14, 4: COLOR 14, 1: PRINT "C";
    COLOR 7, 1: PRINT " - Continuous Update";
  
    LOCATE 15, 4: COLOR 14, 1: PRINT "I";
    COLOR 7, 1: PRINT " - Inputs";
  
    LOCATE 16, 4: COLOR 14, 1: PRINT "O";
    COLOR 7, 1: PRINT " - Output";
   
    LOCATE 17, 4: COLOR 14, 1: PRINT "V";
    COLOR 7, 1: PRINT " - RC Servo";

    LOCATE 18, 4: COLOR 14, 1: PRINT "Q";
    COLOR 7, 1: PRINT " - Quit";
   

    'STATUS.
    COLOR 7, 1
    LOCATE 7, 52: PRINT "    Battery: ";
    LOCATE 8, 52: PRINT "   Whiskers: ";
    LOCATE 9, 52: PRINT "    Compass: ";
    LOCATE 10, 52: PRINT "      Sonar: ";
    LOCATE 11, 52: PRINT "Light Level: ";
    LOCATE 12, 52: PRINT "Sound Level: ";
    LOCATE 13, 52: PRINT "        PIR: ";
    LOCATE 14, 52: PRINT "Temperature: ";
    LOCATE 15, 52: PRINT "       Tilt: ";
   
    RETURN


'--------------------------------------------------------------------
'OPEN THE COMM PORT.
'
'RECEIVES: COMPORT$ AS 1 OR 2.
'
OPENPORT:

    COMPORT$ = "COM" + COMPORT$ + ": 9600,N,8,1"
    OPEN COMPORT$ FOR RANDOM AS #1
    RETURN


'--------------------------------------------------------------------
'GET THE PORT # FROM THE USER.
'
'RESPONSE RETURNS IN COMPORT$
'
GETPORT:

    'GET COM PORT #.
    GOSUB CLEARINPUT
    LOCATE 22, 4: COLOR 7, 1
    PRINT "Select communications port (1 or 2) ";
    DO: COMPORT$ = INKEY$: LOOP UNTIL COMPORT$ <> "": PRINT COMPORT$;
    IF COMPORT$ <> "1" AND COMPORT$ <> "2" THEN GOTO GETPORT
    RETURN


'--------------------------------------------------------------------
'CLEAR THE INPUT BOX.
'
CLEARINPUT:

    COLOR 7, 1
    FOR Y = 21 TO 24
        LOCATE Y, 2
        PRINT SPACE$(78);
    NEXT Y
    RETURN


'--------------------------------------------------------------------
'GET ROBOT STATUS AND DISPLAY.
'
GETSTATUS:
   
    'GET BATTERY VOLTAGE.
    CMND$ = "!1GV1": COUNT = 4: TIMEOUT = 5: GOSUB SEND
    R0$ = LEFT$(RESPONSE$, 2): GOSUB BYTETODEC
    BATTERY$ = STR$(R0) + "."
    R0$ = RIGHT$(RESPONSE$, 2): GOSUB BYTETODEC
    BATTERY$ = BATTERY$ + RIGHT$(STR$(R0), LEN(STR$(R0)) - 1) + " Volts  "
    
    'GET WHISEKRS.
    CMND$ = "!1GW1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    IF RESPONSE$ = "00" THEN WHISKERS$ = "Off" ELSE WHISKERS$ = "On "

    'GET COMPASS.
    CMND$ = "!1GC2": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    IF RESPONSE$ = "00" THEN COMPASS$ = "North     "
    IF RESPONSE$ = "01" THEN COMPASS$ = "North/East"
    IF RESPONSE$ = "02" THEN COMPASS$ = "East      "
    IF RESPONSE$ = "03" THEN COMPASS$ = "South/East"
    IF RESPONSE$ = "04" THEN COMPASS$ = "South     "
    IF RESPONSE$ = "05" THEN COMPASS$ = "South/West"
    IF RESPONSE$ = "06" THEN COMPASS$ = "West      "
    IF RESPONSE$ = "07" THEN COMPASS$ = "North/West"

    'GET SONAR.
    CMND$ = "!1GS1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    R0$ = RESPONSE$: GOSUB BYTETODEC
    SONAR$ = STR$(R0) + CHR$(34) + "  "

    'GET LIGHT LEVEL.
    CMND$ = "!1GL1": COUNT = 8: TIMEOUT = 5: GOSUB SEND
    R0$ = LEFT$(RESPONSE$, 2): GOSUB BYTETODEC
    LIGHTLEVEL$ = STR$(INT(R0 / 2.55)) + "%  "

    'GET SOUND LEVEL.
    CMND$ = "!1GQ1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    R0$ = RESPONSE$: GOSUB BYTETODEC
    SOUNDLEVEL$ = STR$(INT(R0 * 14.28)) + "%  "

    'GET PIR.
    CMND$ = "!1GP1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    IF RESPONSE$ = "00" THEN PIR$ = "Off" ELSE PIR$ = "On "

    'GET TEMPERATURE.
    CMND$ = "!1GF1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    R0$ = RESPONSE$: GOSUB BYTETODEC
    TEMPERATURE$ = STR$(R0) + CHR$(248) + " F  "

    'GET TILT.
    CMND$ = "!1GT1": COUNT = 2: TIMEOUT = 5: GOSUB SEND
    R0$ = RESPONSE$: GOSUB BYTETODEC
    IF RESPONSE$ = "00" THEN TILT$ = "Flat       "
    IF RESPONSE$ = "01" THEN TILT$ = "Front      "
    IF RESPONSE$ = "02" THEN TILT$ = "Right      "
    IF RESPONSE$ = "03" THEN TILT$ = "Back/Right "
    IF RESPONSE$ = "04" THEN TILT$ = "Back       "
    IF RESPONSE$ = "06" THEN TILT$ = "Back/Left  "
    IF RESPONSE$ = "08" THEN TILT$ = "Left       "
    IF RESPONSE$ = "09" THEN TILT$ = "Front/left "
    IF RESPONSE$ = "0F" THEN TILT$ = "Upside down"
    
    GOSUB UPDATE        'UPDATE SCREEN.
    RETURN


'--------------------------------------------------------------------
'UPDATE STATUS ON SCREEN.
'
UPDATE:

    COLOR 14, 1
    LOCATE 7, 64: PRINT BATTERY$;
    LOCATE 8, 65: PRINT WHISKERS$;
    LOCATE 9, 65: PRINT COMPASS$;
    LOCATE 10, 64: PRINT SONAR$;
    LOCATE 11, 64: PRINT LIGHTLEVEL$;
    LOCATE 12, 64: PRINT SOUNDLEVEL$;
    LOCATE 13, 65: PRINT PIR$;
    LOCATE 14, 64: PRINT TEMPERATURE$;
    LOCATE 15, 65: PRINT TILT$;
    RETURN


'--------------------------------------------------------------------
'SEND COMMANDS TO TRILOBOT.
'
'CMND$ = COMMAND.
'COUNT = RESPONSE CHARACTER COUNT.
'TIMEOUT = TIME TO WAIT FOR TRANSMIT OR RESPONSE.
'RESPONSE$= THE RESPONSE.
'
SEND:

    RESPONSE$ = ""          'INITIALIZE RESPONSE.
  
    'CLEAR INPUT BUFFER.
    IF LOC(1) > 0 THEN X$ = INPUT$(LOC(1), #1)

    'SEND COMMAND.
    PRINT #1, CHR$(27) + CMND$;
      
    'WAIT UNTIL TRANSMIT BUFFER IS EMPTY.
    T = TIMER
    DO
    IF TIMER > T + TIMEOUT THEN
        'TIMEOUT HAS OCCURED.
        BEEP
        LOCATE 21, 4
        COLOR 7, 1
        INPUT "Communication transmit error, Press ENTER "; HUH$
        RETURN
    END IF
    LOOP UNTIL LOF(1) = 512

    'GET CHARACTERS FROM TRILOBOT.
    IF COUNT > 0 THEN
        T = TIMER
        DO
        IF TIMER > T + TIMEOUT THEN
            'TIMEOUT HAS OCCURED.
            BEEP
            LOCATE 21, 4
            COLOR 7, 1
            INPUT "Trilobot did not respond to command, Press ENTER "; HUH$
            PRINT #1, CHR$(27);     'SEND ESCAPE TO RESET ALL CONTROLLERS.
            RETURN
        END IF
        LOOP UNTIL LOC(1) = COUNT
        RESPONSE$ = INPUT$(COUNT, #1)
    END IF
   
    RETURN


'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