'************************************************************************
'*                   PROGRAM NAME:   ROBOT1.BAS                         *
'*                         AUTHOR:   Torrey Horn                        *
'*                    LAST UPDATE:   12/10                              *
'*                                                                      *
'*   This program is meant to provide an easy-to-use interface for the  *
'*   use of the ARMDROID I robot arm.  It will provide a means of       *
'*   directing the robot arm and editing and saving commands for the    *
'*   robot arm.                                                         *
'*                                                                      *
'************************************************************************
DECLARE SUB addlist (mot1$, dir1$, dis1$)
DECLARE SUB clearsel ()
DECLARE SUB copy ()
DECLARE SUB cut ()
DECLARE SUB delayset ()
DECLARE SUB dellist (curr%)
DECLARE SUB directmode ()
DECLARE SUB displaylist ()
DECLARE SUB displaylist2 ()
DECLARE SUB displaylist3 ()
DECLARE SUB editmode ()
DECLARE SUB executeit (comm$)
DECLARE SUB exitit ()
DECLARE SUB getselection ()
DECLARE SUB initdata ()
DECLARE SUB initscreen ()
DECLARE SUB inslist (mot1$, dir1$, dis1$)
DECLARE SUB insmode ()
DECLARE SUB keyoff ()
DECLARE SUB keyon ()
DECLARE SUB menudisplay ()
DECLARE SUB openit ()
DECLARE SUB paste ()
DECLARE SUB playbackmode ()
DECLARE SUB printerset ()
DECLARE SUB printit ()
DECLARE SUB recordmode ()
DECLARE SUB resetarm ()
DECLARE SUB resetarray ()
DECLARE SUB resetcount ()
DECLARE SUB robotset ()
DECLARE SUB saveit ()
DECLARE SUB scroll (direc%)
DECLARE SUB selection (dir%)


CONST arraymax = 400          'maximum array size
CONST vmode = 0               'video mode (0 for everything but Hercules (3))
CONST foreground = 7          'text color    (7=white)
CONST backround = 1           'backround color (1=blue, 0=black)

TYPE modetype
  direct AS STRING * 1        'direct mode (Y or N)
  record AS STRING * 1        'record mode (Y or N)
  edit AS STRING * 1          'edit mode (Y or N)
  playback AS STRING * 1      'playback mode (Y or N)
  ins AS STRING * 1           'insert (Y or N)
  printer AS STRING * 1       'printer connected (Y or N)
  port1 AS STRING * 5         'LPT1: or LPT2:
  port1add AS INTEGER         'port address - &h378 or &h278
  arm AS STRING * 1           'robot connected (Y or N)
  port2 AS STRING * 5
  port2add AS INTEGER
  delay AS STRING * 5         'delay count
END TYPE

TYPE offsettype               'offsets from original position
  b AS INTEGER
  g AS INTEGER
  f AS INTEGER
  R AS INTEGER
  s AS INTEGER
  w AS INTEGER
END TYPE

TYPE menumode
   active AS INTEGER         '1=yes 0=no 2=tab active
   menunum AS INTEGER        'which menu
   sel AS INTEGER            'which selection
   sub1 AS INTEGER           'which subselection
END TYPE

TYPE commandrec              'linked list of commands
    mot AS STRING * 1            'motor
    dir AS STRING * 1            'direction
    dis AS STRING * 3            'distance
    used AS INTEGER              'used node (1=yes)
    next1 AS INTEGER             'next node
    last1 AS INTEGER             'previous node
END TYPE

TYPE listtype                'linked list info
    head AS INTEGER
    tail AS INTEGER
    current AS INTEGER       'current node
    look1 AS INTEGER         'selection start
    look2 AS INTEGER         'selection end
END TYPE

COMMON SHARED llist AS listtype
COMMON SHARED mode AS modetype
COMMON SHARED offset AS offsettype
COMMON SHARED menus1 AS menumode
COMMON SHARED line1$, line2$, line3$, line4$, line5$, line6$, line7$, blank1$
COMMON SHARED erroroccured AS INTEGER, enterpressed AS INTEGER
COMMON SHARED commarray() AS commandrec
COMMON SHARED clip() AS commandrec, clipcount AS INTEGER
COMMON SHARED robotarray() AS INTEGER, sellist() AS INTEGER, bigx%

DIM robotarray(1 TO 8) AS INTEGER                  'stepper motor numbers
DIM sellist(arraymax) AS INTEGER                   'selected nodes from llist
DIM commarray(arraymax) AS commandrec              'linked list of commands
DIM clip(arraymax) AS commandrec                   'clipboard

'=======================  Key Definitions =======================
ON ERROR GOTO errorhandler           'call 911 routines
ON KEY(1) GOSUB end1                 'F1 - emergency exit
ON KEY(9) GOSUB f9: KEY(9) ON        'F9 - linked list info - debugging
ON KEY(10) GOSUB f10: KEY(10) ON     'F10 - clipboard info  - debugging

ON KEY(11) GOSUB up                  'keypad up arrow
ON KEY(12) GOSUB left                'keypad left arrow
ON KEY(13) GOSUB right               'keypad right arrow
ON KEY(14) GOSUB down                'keypad down arrow

KEY 15, CHR$(128) + CHR$(72)         'dedicated up arrow on 101 keyboard
ON KEY(15) GOSUB up

KEY 16, CHR$(128) + CHR$(80)         'dedicated down arrow
ON KEY(16) GOSUB down

KEY 17, CHR$(128) + CHR$(75)         'dedicated left arrow
ON KEY(17) GOSUB left

KEY 18, CHR$(128) + CHR$(77)         'dedicated right arrow
ON KEY(18) GOSUB right

KEY 19, CHR$(0) + CHR$(1)            'ESC  key
ON KEY(19) GOSUB esc

KEY 20, CHR$(0) + CHR$(28)           'ENTER  key
ON KEY(20) GOSUB enter

KEY 21, CHR$(0) + CHR$(15)           'TAB  key
ON KEY(21) GOSUB tab1

KEY 22, CHR$(1) + CHR$(72)           'Shift + up  key
ON KEY(22) GOSUB shiftup

KEY 23, CHR$(1) + CHR$(80)           'Shift + down  key
ON KEY(23) GOSUB shiftdown

KEY 24, CHR$(129) + CHR$(72)         'Shift + dedicated up  key
ON KEY(24) GOSUB shiftup

KEY 25, CHR$(129) + CHR$(80)         'Shift + dedicated down  key
ON KEY(25) GOSUB shiftdown

'===========================  MAIN PROGRAM  ==============================

CLS
CALL keyon                        'enable keytrapping
CALL initdata                     'initialize data
CALL initscreen                   'initialize screen

count1 = 1                        'number of keys entered
dis1$ = ""                        'clear distance string

DO                                'loop forever
  a$ = ""
  DO WHILE a$ = ""
     IF count1 > 5 THEN enterpressed = 1         'if all positions filled

     IF enterpressed = 1 AND count1 > 3 THEN     'if enter pressed legally
        enterpressed = 0                         'reset variable
        comm$ = mot1$ + dir1$ + MID$(dis1$, 1, 3)
        IF mode.record = "Y" THEN                 'if recording
           IF mode.ins = "N" THEN
              CALL addlist(mot1$, dir1$, dis1$)
           ELSE
               CALL inslist(mot1$, dir1$, dis1$)
           END IF
        END IF
        IF mode.direct = "Y" THEN CALL executeit(comm$)
        LOCATE 14, 42: PRINT "   ";              'clear area
        LOCATE 14, 26: PRINT " ";
        LOCATE 14, 9: PRINT " ";
        count1 = 1
        dis1$ = ""
     ELSE
        enterpressed = 0               'too keep from taking too few digits
     END IF

  a$ = INKEY$
  LOOP
                                       'check for legal commands
  IF INSTR("BGFRSW", UCASE$(a$)) > 0 AND count1 = 1 THEN
    mot1$ = UCASE$(a$)
    count1 = 2
    LOCATE 14, 9
    PRINT mot1$;
    a$ = " "                    'to keep from getting RR's all the time
  END IF

  IF INSTR("UDLROC+-", UCASE$(a$)) > 0 AND count1 = 2 THEN
    dir1$ = UCASE$(a$)
    count1 = 3
    LOCATE 14, 26
    PRINT dir1$;
    a$ = " "
  END IF

  IF (a$ >= "0" AND a$ <= "9") AND count1 > 2 THEN
     dis1$ = dis1$ + a$
     LOCATE 14, 42
     PRINT dis1$;
     count1 = count1 + 1
  END IF
LOOP

END                                     'END OF THE PROGRAM


'======================  EVENT HANDLERS  ==========================

up:                                       'if UP key pressed
  IF menus1.active = 1 THEN
     menus1.sel = menus1.sel - 1
     IF menus1.sel = 0 THEN
        IF menus1.menunum = 4 OR menus1.menunum = 5 THEN
           menus1.sel = 3
        ELSE
           menus1.sel = 4
        END IF
     END IF
     CALL menudisplay
  ELSE
    IF menus1.active = 2 THEN          'if TAB active
        CALL clearsel
        CALL scroll(-1)
    END IF
  END IF

RETURN

down:                                     'if DOWN key pressed
  IF menus1.active = 1 THEN
     menus1.sel = menus1.sel + 1
     IF menus1.sel = 4 THEN
        IF menus1.menunum = 4 OR menus1.menunum = 5 THEN
           menus1.sel = 5
        ELSE
           menus1.sel = 4
        END IF
     END IF
     IF menus1.sel = 5 THEN menus1.sel = 1
     CALL menudisplay
  ELSE
    IF menus1.active = 2 THEN      'if TAB active
        CALL clearsel
        CALL scroll(1)
    END IF
  END IF
RETURN

left:                                     'if LEFT key pressed
  IF menus1.active = 1 THEN
     menus1.sel = 1
     menus1.menunum = menus1.menunum - 1
     IF menus1.menunum = 0 THEN
        menus1.menunum = 5
     END IF
     CALL menudisplay
  ELSE
  END IF
RETURN

right:                                    'if RIGHT key pressed
  IF menus1.active = 1 THEN
     menus1.sel = 1
     menus1.menunum = menus1.menunum + 1
     IF menus1.menunum = 6 THEN
        menus1.menunum = 1
     END IF
     CALL menudisplay
  ELSE

  END IF
RETURN

end1:                                  'emergency exit routine
    END
RETURN

esc:                                   'if ESC key pressed
  IF menus1.active = 0 THEN
     menus1.active = 1
     menus1.menunum = 1
     menus1.sel = 1
  ELSE
     IF menus1.active = 2 THEN
         menus1.active = 1
         menus1.menunum = 1
         menus1.sel = 1
         LOCATE 2, 75: PRINT "   ";
     ELSE
         menus1.active = 0
     END IF
  END IF

  IF menus1.active = 1 THEN
     CALL menudisplay
  ELSE
     SCREEN vmode, , 0, 0
     COLOR foreground, backround
  END IF
RETURN

enter:                                  'if ENTER key pressed
  IF menus1.active = 0 THEN
      enterpressed = 1
  ELSE
     IF menus1.active = 1 THEN
        CALL getselection
     END IF
  END IF

RETURN

tab1:                                 'pressed TAB to get to/from command list
  IF menus1.active = 0 THEN
     menus1.active = 2
     LOCATE 2, 75: PRINT "TAB";
     sellist(llist.current) = 1
  ELSE menus1.active = 0
     LOCATE 2, 75: PRINT "   ";
     CALL clearsel
  END IF
  CALL displaylist
RETURN

shiftup:
  IF menus1.active = 2 AND mode.edit = "Y" THEN CALL selection(0)
RETURN
shiftdown:
  IF menus1.active = 2 AND mode.edit = "Y" THEN CALL selection(1)
RETURN


errorhandler:
  erroroccured = 1
  IF ERR = 53 THEN                             'File not found error
     LOCATE 19, 12
     COLOR backround, foreground
     PRINT "          **  FILE NOT FOUND **            ";
     COLOR foreground, backround
     FOR i = 1 TO 15000
     NEXT i
  ELSE
     LOCATE 19, 12
     PRINT "  ** Error"; ERR; " has occurred. **  ";
     FOR i = 1 TO 15000
     NEXT i
  END IF
RESUME NEXT

f9:                          'display listing of array - for debugging
CALL displaylist2
RETURN

f10:                         'display clipboard - for debugging
CALL displaylist3
RETURN

END

SUB addlist (mot1$, dir1$, dis1$)
'************************************************************************
'*  SUB:  addlist                                                       *
'*                                                                      *
'*  Purpose: Add a command to the command linked-list in overtype mode. *
'************************************************************************
curr1% = llist.current                       'current pointer
oldcurr% = commarray(curr1%).last1           'old current pointer

  lastptr% = commarray(curr1%).next1         'pointer to last node
  findcurr% = curr1% + 1                     'new current pointer
  commarray(curr1%).mot = mot1$
  commarray(curr1%).dir = dir1$
  commarray(curr1%).dis = dis1$

  commarray(curr1%).used = 1
  WHILE commarray(findcurr%).used <> 0
        findcurr% = findcurr% + 1
        IF findcurr% > arraymax THEN findcurr% = 1
  WEND
  IF lastptr% = 0 THEN                        'if last in the list
      commarray(curr1%).next1 = findcurr%
      commarray(curr1%).last1 = oldcurr%
      oldcurr% = curr1%
      IF lastptr% = 0 THEN llist.tail = curr1%
      commarray(findcurr%).next1 = lastptr%
      commarray(findcurr%).last1 = curr1%
      llist.current = findcurr%
  ELSE                                        'else (if middle) do nothing
     llist.current = commarray(curr1%).next1
     commarray(findcurr%).next1 = 0
  END IF

CALL clearsel                        'clear all previous selections
llist.look1 = llist.current          'highlight current entry
CALL displaylist                     'show it
quitaddlist:

END SUB

SUB clearsel
'************************************************************************
'*  SUB:  clearsel                                                      *
'*                                                                      *
'*  Purpose:   Clears the selection array (sellist).                    *
'************************************************************************

FOR i% = 0 TO arraymax
sellist(i%) = 0                   'clear all previous selections
NEXT i%
sellist(llist.current) = 1        'except for current entry
END SUB

SUB copy
'************************************************************************
'*  SUB:  copy                                                          *
'*                                                                      *
'*  Purpose:  Copy selected command(s) to clipboard.                    *
'************************************************************************
IF mode.edit = "N" THEN GOTO quitcopy

IF llist.look1 = llist.current AND commarray(llist.current).used = 0 THEN
     GOTO quitcopy
END IF

IF llist.look1 = llist.current THEN         'if only current node selected
   clip(1).mot = commarray(llist.look1).mot
   clip(1).dir = commarray(llist.look1).dir
   clip(1).dis = commarray(llist.look1).dis
   clip(1).used = llist.look1                 'used to keep track of
                                              'original node number
   clipcount = 1                              'number of objects in clip
ELSE                                       'more than one selected item
   temp1% = llist.look1
   temp2% = llist.current
   clipcount = 0
   DO WHILE temp1% <> temp2%                                 'get them all
      clipcount = clipcount + 1
      clip(clipcount).mot = commarray(temp1%).mot
      clip(clipcount).dir = commarray(temp1%).dir
      clip(clipcount).dis = commarray(temp1%).dis
      clip(clipcount).used = temp1%
      temp1% = commarray(temp1%).next1
   LOOP
   IF temp1% = temp2% AND commarray(temp2%).used = 1 THEN     'get last one
      clipcount = clipcount + 1
      clip(clipcount).mot = commarray(temp1%).mot
      clip(clipcount).dir = commarray(temp1%).dir
      clip(clipcount).dis = commarray(temp1%).dis
      clip(clipcount).used = temp1%
   END IF
END IF

quitcopy:

menus1.active = 0
CALL menudisplay

END SUB

SUB cut
'************************************************************************
'*  SUB:  cut                                                           *
'*                                                                      *
'*  Purpose:  Copy a command(s) to clipboard and delete from list.      *
'************************************************************************
IF mode.edit = "N" THEN GOTO quitcut
CALL copy                           'first copy all selected items to clip
FOR i% = 1 TO clipcount
   curr% = clip(i%).used
   CALL dellist(curr%)              'then delete them from the original
NEXT i%
CALL clearsel
CALL displaylist
quitcut:
menus1.active = 0
CALL menudisplay

END SUB

SUB delayset
'************************************************************************
'*  SUB:  delayset                                                      *
'*                                                                      *
'*  Purpose: Set the delay (between commands) variable from menu.       *
'************************************************************************

COLOR backround, foreground
LOCATE 7, 45
PRINT " The current delay is:  ";
PRINT mode.delay;

LOCATE 8, 45
PRINT " Enter another delay (0-999) ";
LOCATE 9, 45
PRINT "   or hit <ESC>.             ";

CALL keyoff
del$ = ""
FOR i = 1 TO 3
a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN GOTO quitdelay
IF a$ >= "0" AND a$ <= "9" THEN del$ = del$ + a$
LOCATE 11, 50
PRINT del$;
NEXT i

IF del$ <> "" THEN                         'write out new information
    mode.delay = del$
    OPEN "setup.inf" FOR OUTPUT AS #1      'to setup.inf file.
      PRINT #1, mode.printer
      PRINT #1, mode.port1
      PRINT #1, mode.arm
      PRINT #1, mode.port2
      PRINT #1, mode.delay
    CLOSE #1
END IF

quitdelay:
COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay

END SUB

SUB dellist (curr%)
'************************************************************************
'*  SUB:  dellist                                                       *
'*                                                                      *
'*  Purpose:  Delete an item from the list.                             *
'************************************************************************


IF commarray(curr%).used = 0 THEN GOTO quitdel       'if empty, dont del

lastptr% = commarray(curr%).last1        'connect preceding to succeeding
nextptr% = commarray(curr%).next1
commarray(lastptr%).next1 = nextptr%
commarray(nextptr%).last1 = lastptr%
commarray(curr%).mot = " "               'clear all fields
commarray(curr%).dir = " "
commarray(curr%).dis = " "
commarray(curr%).used = 0
commarray(curr%).next1 = 0
commarray(curr%).last1 = 0
IF llist.head <> llist.tail THEN
   IF curr% = llist.tail THEN llist.tail = lastptr%
   IF curr% = llist.head THEN llist.head = nextptr%
END IF
llist.current = nextptr%
quitdel:
END SUB

SUB directmode
'************************************************************************
'*  SUB:  directmode                                                    *
'*                                                                      *
'*  Purpose: Change to and from the DIRECT MODE.                        *
'************************************************************************

IF mode.direct = "Y" THEN
   mode.direct = "N"
ELSE
   mode.direct = "Y"
END IF
menus1.active = 0
CALL menudisplay

END SUB

SUB displaylist
'************************************************************************
'*  SUB:  displaylist                                                   *
'*                                                                      *
'*  Purpose:  Displays the entries in the command listing according     *
'*            to the current node in the linked list.                   *
'************************************************************************


SCREEN vmode, , 0, 0                  'get to proper screen
COLOR foreground, backround
FOR i = 1 TO 14                       'clear portion of screen
LOCATE i + 3, 64: PRINT "      ";
NEXT i

curr1% = llist.current
i = 0
IF commarray(llist.head).used = 1 THEN
   SCREEN vmode, , 0, 0
   COLOR backround, foreground

   comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
   LOCATE 17 - i, 65
   IF comm$ = "     " AND commarray(curr1%).next1 = 0 THEN
      PRINT "*END*";
   ELSE
      PRINT comm$;
   END IF
   COLOR foreground, backround
   DO
   i = i + 1
   curr1% = commarray(curr1%).last1
   comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
   LOCATE 17 - i, 65
   IF sellist(curr1%) = 1 THEN COLOR backround, foreground
   PRINT comm$;
   COLOR foreground, backround
   LOOP UNTIL (commarray(curr1%).last1 = 0 OR curr1% = llist.head OR i > 12)
END IF

quitdisplaylist:

END SUB

SUB displaylist2
'************************************************************************
'*  SUB:  displaylist2                                                  *
'*                                                                      *
'*  Purpose:  Displays the entries in the command listing according     *
'*            to the current node in the linked list.  Also linked      *
'*            list information (technical) when F9 is pressed.          *
'************************************************************************

keyoff
SCREEN vmode, , 2, 2
COLOR foreground, backround
CLS
LOCATE 1, 1
PRINT "Command Array", "Current:"; llist.current, "Head:"; llist.head, "Tail:"; llist.tail, "Look1:"; llist.look1
PRINT

PRINT " #", "Command", "Last1", "Next1", "Selected"
IF llist.current > llist.tail THEN
   lastq% = llist.current
ELSE
   lastq% = llist.tail
END IF

FOR curr1% = llist.head TO lastq% + 1
comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
PRINT curr1%, comm$, commarray(curr1%).last1, commarray(curr1%).next1, sellist(curr1%)
NEXT curr1%
SLEEP
SCREEN vmode, , 0, 0
COLOR foreground, backround

keyon
END SUB

SUB displaylist3
'************************************************************************
'*  SUB:  displaylist3                                                  *
'*                                                                      *
'*  Purpose:  Displays the entries in the clipboard.                    *
'*              When F10 is pressed.                                    *
'************************************************************************

keyoff
SCREEN vmode, , 2, 2
COLOR foreground, backround
CLS
LOCATE 1, 1
PRINT "Clipboard", , "Clipcount: "; clipcount
PRINT " #", "Command", "Last1", "Next1", "Used"
FOR curr1% = 1 TO clipcount
comm$ = clip(curr1%).mot + clip(curr1%).dir + clip(curr1%).dis
PRINT curr1%, comm$, clip(curr1%).last1, clip(curr1%).next1, clip(curr1%).used
NEXT curr1%
SLEEP
SCREEN vmode, , 0, 0
COLOR foreground, backround

keyon

END SUB

SUB editmode

'************************************************************************
'*  SUB:  editmode                                                      *
'*                                                                      *
'*  Purpose:  Change to and from the EDIT MODE.                         *
'************************************************************************

IF mode.edit = "Y" THEN
   mode.edit = "N"
ELSE
   mode.edit = "Y"
END IF
menus1.active = 0
CALL menudisplay


END SUB

SUB executeit (comm$)
'************************************************************************
'*  SUB:  executeit                                                     *
'*                                                                      *
'*  Purpose:  Heart of the program.  Takes a command string and breaks  *
'*            it up into motor, direction and distance components.      *
'*            It then sends the proper binary command to the robot arm  *
'*            and keeps track of the counter offset settings.           *
'************************************************************************

IF (mode.direct = "N" OR mode.port2 = "N") THEN GOTO quitexecute

mot1$ = MID$(comm$, 1, 1)             'motor is 1st character
dir1$ = MID$(comm$, 2, 1)             'direction is 2nd character
dis1$ = MID$(comm$, 3, 3)             'distance is next 3 characters
dist1% = VAL(dis1$)                   'numeric value of distance
delay1% = VAL(mode.delay)             'numeric value of delay

SELECT CASE mot1$                     'which motor is it?
   CASE "G"
      m1% = 1                         'gripper
   CASE "F"
      m1% = 4                         'forearm
   CASE "S"
      m1% = 5                         'shoulder
   CASE "R"
      m1% = 3                         'Right wrist roll
   CASE "W"
      m1% = 2                         'Left wrist roll
   CASE "B"
      m1% = 6                         'base
   CASE ELSE
      GOTO quitexecute
END SELECT

SELECT CASE dir1$                   'is direction positive or negative?
   CASE "D"
     IF mot1$ = "S" THEN d1% = 0 ELSE IF mot1$ = "R" THEN d1% = 0 ELSE d1% = 1
   CASE "U"
     IF mot1$ = "S" THEN d1% = 1 ELSE IF mot1$ = "R" THEN d1% = 1 ELSE d1% = 0
   CASE "L"
      d1% = 0
   CASE "R"
      d1% = 1
   CASE "O"
      d1% = 1
   CASE "C"
      d1% = 0
   CASE "F"
      d1% = 1
   CASE "B"
      d1% = 0
   CASE "+"
      d1% = 1
   CASE "-"
      d1% = 0
END SELECT

DIM Z1 AS INTEGER
DIM Z2 AS INTEGER

O% = m1% + m1% + 1                    'shift motor over 1 bit and
                                      'set low order bit to a 1
x% = bigx%                            'remember where we are in array

FOR y% = 1 TO dist1%                  'perform dist1% steps

  Z1 = (O% + robotarray(x%))          'add motor to robot array (step #)
  Z2 = (O% + robotarray(x%) - 1)      'same as z1 but with 0 in low bit

  FOR q = 1 TO delay1%                'delay between commands
  NEXT q

  DEF SEG = 0                         'define segment zero
  OUT mode.port2add, Z1
  OUT mode.port2add, Z2
  'POKE mode.port2add, z1             'send z1 into LPT1: or LPT2:
  'POKE mode.port2add, z2             'send z2 into same port to latch it
  DEF SEG                             'return to default segment

  IF d1% = 1 THEN                     'determine direction (Pos or Neg)
     x% = x% - 1
     IF x% < 1 THEN x% = 8
  ELSE
     x% = x% + 1
     IF x% > 8 THEN x% = 1
  END IF
NEXT y%                                'end of distance loop

bigx% = x%                             'remember where in the array we are.


SELECT CASE mot1$                      'adjust offset counters
   CASE "G"     '**  GRIPPER **
      IF d1% = 0 THEN
          offset.g = offset.g - dist1%
      ELSE
          offset.g = offset.g + dist1%
      END IF

   CASE "F"     '** FOREARM **
      IF d1% = 0 THEN
          offset.f = offset.f + dist1%
      ELSE
          offset.f = offset.f - dist1%
      END IF

   CASE "S"     '** SHOULDER **
      IF d1% = 0 THEN
          offset.s = offset.s - dist1%
      ELSE
          offset.s = offset.s + dist1%
      END IF

   CASE "R"     '** RIGHT WRIST **
      IF d1% = 0 THEN
          offset.R = offset.R - dist1%
      ELSE
          offset.R = offset.R + dist1%
      END IF

   CASE "W"     '** LEFT WRIST **
      IF d1% = 0 THEN
          offset.w = offset.w + dist1%
      ELSE
          offset.w = offset.w - dist1%
      END IF

   CASE "B"     '** ROBOT BASE **
      IF d1% = 0 THEN
          offset.b = offset.b - dist1%
      ELSE
          offset.b = offset.b + dist1%
      END IF

   CASE ELSE
      GOTO quitexecute
END SELECT

LOCATE 23, 1                            'rewrite offset counters
PRINT blank1$;
LOCATE 23, 1
PRINT "B: "; offset.b; "    G: "; offset.g; "    F: "; offset.f; "    R: "; offset.R; "    S: "; offset.s; "    W: ";
offset.w;

quitexecute:
END SUB

SUB exitit
'************************************************************************
'*  SUB:  exitit                                                        *
'*                                                                      *
'*  Purpose:  Exits program.  Saves any data.                           *
'************************************************************************

         CLS
         CALL keyoff
         IF llist.head <> llist.tail THEN
            LOCATE 4, 15
            INPUT " Would you like to save your data? (Y or N): "; ans$
            IF ans$ = "Y" OR ans$ = "y" THEN CALL saveit
         END IF
         SCREEN vmode, , 1, 1
         CLS
         SCREEN vmode, , 0, 0
         CLS
         LOCATE 10, 20
         PRINT "Do not forget to turn off the robot-arm."
         LOCATE 12, 22
         PRINT "         Have a nice day!               "
         LOCATE 16, 25
         
         PRINT "Press any key to continue..."
         SLEEP
         CLS
         END                        'Quit

END SUB

SUB getselection
'************************************************************************
'*  SUB:  getselection                                                  *
'*                                                                      *
'*  Purpose:  Determines which item is selected from the menu.          *
'************************************************************************

SELECT CASE menus1.menunum         'decide which menu and selection
   CASE 1
      SELECT CASE menus1.sel
      CASE 1
          CALL openit
      CASE 2
          CALL saveit
      CASE 3
          CALL printit
      CASE 4
          CALL exitit
      END SELECT
   CASE 2
      SELECT CASE menus1.sel
      CASE 1
          CALL directmode
      CASE 2
          CALL recordmode
      CASE 3
          CALL playbackmode
      CASE 4
          CALL editmode
      END SELECT
   CASE 3
      SELECT CASE menus1.sel
      CASE 1
          CALL cut
      CASE 2
          CALL copy
      CASE 3
          CALL paste
      CASE 4
          CALL insmode
      END SELECT
   CASE 4
      SELECT CASE menus1.sel
      CASE 1
          CALL delayset
      CASE 2
          CALL printerset
      CASE 3
          CALL robotset
      END SELECT
   CASE 5
      SELECT CASE menus1.sel
      CASE 1
          CALL resetarm
      CASE 2
          CALL resetcount
      CASE 3
          CALL resetarray
      END SELECT
END SELECT
END SUB

SUB initdata
'************************************************************************
'*  SUB:  initdata                                                      *
'*                                                                      *
'*  Purpose:  Initializes all beginning global data and sets keyboard.  *
'************************************************************************

'=======================   Initialize data  ===========================

DEF SEG = 0                           'force off CAPS LOCK, SCROLL LOCK,
POKE 1047, 0                          'NUM LOCK and INSERT keys so as not
DEF SEG                               'to affect keytrapping

COLOR foreground, backround           'set original color

offset.b = 0                          'set offsets to 0
offset.g = 0
offset.f = 0
offset.R = 0
offset.s = 0
offset.w = 0
  
bigx% = 1                             'set initial array counter

robotarray(1) = 192                   'initialize step numbers
robotarray(2) = 144                   'for single-stepping motors
robotarray(3) = 48
robotarray(4) = 96
robotarray(5) = 192
robotarray(6) = 144
robotarray(7) = 48
robotarray(8) = 96

menus1.active = 0                    'set menus to inactive
menus1.menunum = 1                   'set first menu to File
menus1.sel = 1                       'first selection
menus1.sub1 = 0

FOR i% = 1 TO arraymax               'init linked-list (blanks)
commarray(i%).mot = " "
commarray(i%).dir = " "
commarray(i%).dis = " "
commarray(i%).used = 0
commarray(i%).next1 = 0
commarray(i%).last1 = 0
NEXT i%

llist.head = 1                   'head of linked list
llist.tail = 1                   'tail of linked list
llist.current = 1                'current node in linked list
llist.look1 = 1                  'first node in selection list
llist.look2 = 0                  'direction of selection (up or down)
clipcount = 0                    'number of items in clipboard

mode.direct = "Y"                'set initial modes
mode.record = "N"
mode.playback = "N"
mode.edit = "Y"
mode.ins = "N"


OPEN "setup.inf" FOR INPUT AS #1                'read in setup.inf file

IF erroroccured = 1 OR LOF(1) < 10 THEN         'if file is not found
   mode.printer = "N"
   mode.port1 = "     "
   mode.arm = "N"
   mode.port2 = "     "
   mode.delay = "999"
   erroroccured = 0
ELSE                                             'if file is found
  INPUT #1, mode.printer
  INPUT #1, mode.port1
  INPUT #1, mode.arm
  INPUT #1, mode.port2
  INPUT #1, mode.delay
END IF
CLOSE #1
                                                     'set port addresses
IF mode.port1 = "LPT1:" THEN mode.port1add = &H378
IF mode.port1 = "LPT2:" THEN mode.port1add = &H278
IF mode.port2 = "LPT1:" THEN mode.port2add = &H378
IF mode.port2 = "LPT2:" THEN mode.port2add = &H278
IF mode.port2 <> "LPT1:" OR mode.port2 <> "LPT2:" THEN mode.port2add = &H378

                                         'create different symbols for screen
line1$ = CHR$(196)                       'solid underlines (diff lengths)
line2$ = CHR$(95) + CHR$(95) + CHR$(95)
line3$ = line1$
FOR i% = 1 TO 79
  line1$ = line1$ + CHR$(196)
NEXT i%
line4$ = CHR$(179)                       'solid downbar
line5$ = CHR$(195)                       'solid downbar w/cross
line6$ = CHR$(192)                       'corner cross
line7$ = CHR$(194)                       't cross
blank1$ = " "
FOR i = 1 TO 78                          'blanks
blank1$ = blank1$ + " "
NEXT i

END SUB

SUB initscreen
'************************************************************************
'*  SUB:  initscreen                                                    *
'*                                                                      *
'*  Purpose:  Displays first screen of program.                         *
'************************************************************************

SCREEN vmode, , 0, 0
COLOR foreground, backround
CLS
LOCATE 1, 1
PRINT line1$;
LOCATE 2, 1
PRINT " File     Mode     Edit     Setup     Reset             Command Listing:";
LOCATE 3, 1
PRINT line1$;

LOCATE 4, 1
PRINT " Motor:          Direction:       Distance (steps)  ";
PRINT
LOCATE 6, 1
PRINT " B=Base          L=Left         (0-999) enter 3 digits!"
LOCATE 7, 1
PRINT " S=Shoulder      R=Right        like 050, 100, 012 ";
LOCATE 8, 1
PRINT " F=Forearm       U=Up D=Down         ";
LOCATE 9, 1
PRINT " R=Right Wrist   F=Forward B=Backward ";
LOCATE 10, 1
PRINT " W=Left Wrist    O=Open";
LOCATE 11, 1
PRINT " G= Gripper      C=Close";
LOCATE 12, 1
PRINT "             use  + or - instead of U,D,F,B,L,R,O,C   ";
PRINT "                       if you like:; "
LOCATE 15, 1
PRINT "  Mot: "; line2$; "         Dir: "; line2$; "        Dist: "; line2$;

LOCATE 18, 1
PRINT line1$;
LOCATE 19, 1
PRINT "Messages:";
LOCATE 22, 1
PRINT line1$;
LOCATE 24, 1
PRINT line1$;
LOCATE 21, 1
PRINT "Active Modes --->   Direct: "; mode.direct; "    Record: "; mode.record; "   Insert: "; mode.ins; "    Edit: ";
Mode.edit;

LOCATE 23, 1
PRINT "B: "; offset.b; "    G: "; offset.g; "    F: "; offset.f; "    R: "; offset.R; "    S: "; offset.s; "    W: ";
offset.w;

LOCATE 19, 12
COLOR backround, foreground
PRINT " Enter a command.   <ESC> = Menus/cancel,  <TAB> = Command List. ";
COLOR foreground, backround
LOCATE 17, 71: PRINT "<-Current";
LOCATE 3, 58: PRINT line7$;
FOR i = 4 TO 17
LOCATE i, 58
PRINT line4$
NEXT i
LOCATE 18, 58: PRINT CHR$(193);

END SUB

SUB inslist (mot1$, dir1$, dis1$)
'************************************************************************
'*  SUB:  inslist                                                       *
'*                                                                      *
'*  Purpose:  Add a command to the command linked-list in insert mode.  *
'************************************************************************

curr1% = llist.current                       'current pointer
oldcurr% = commarray(curr1%).last1           'old current pointer

  lastptr% = commarray(curr1%).next1         'pointer to last node
  findcurr% = curr1% + 1                     'new current pointer
  WHILE commarray(findcurr%).used <> 0       'find something empty
        findcurr% = findcurr% + 1
        IF findcurr% > arraymax THEN findcurr% = 1
  WEND
  IF lastptr% = 0 THEN                    'if last in the list, no problem
      commarray(curr1%).mot = mot1$
      commarray(curr1%).dir = dir1$
      commarray(curr1%).dis = dis1$
      commarray(curr1%).used = 1
      commarray(curr1%).next1 = findcurr%
      commarray(curr1%).last1 = oldcurr%
      oldcurr% = curr1%
      IF lastptr% = 0 THEN llist.tail = curr1%
      commarray(findcurr%).next1 = lastptr%
      commarray(findcurr%).last1 = curr1%
      llist.current = findcurr%
  ELSE                                    'else (if middle) get new node
      commarray(findcurr%).mot = mot1$
      commarray(findcurr%).dir = dir1$
      commarray(findcurr%).dis = dis1$
      commarray(findcurr%).used = 1

      commarray(findcurr%).next1 = curr1%
      commarray(findcurr%).last1 = oldcurr%
      commarray(curr1%).last1 = findcurr%
      commarray(oldcurr%).next1 = findcurr%
      findcurr2% = findcurr%

      WHILE commarray(findcurr2%).used <> 0
            findcurr2% = findcurr2% + 1
            IF findcurr2% > arraymax THEN findcurr2% = 1
      WEND
      commarray(findcurr2%).last1 = llist.tail
      commarray(llist.tail).next1 = findcurr2%
  END IF

CALL clearsel                           'clear all previous selections
llist.look1 = llist.current             'select current item
CALL displaylist

END SUB

SUB insmode
'************************************************************************
'*  SUB:  insmode                                                       *
'*                                                                      *
'*  Purpose: Change from overtype mode to insert mode.                  *
'************************************************************************
IF mode.ins = "Y" THEN
   mode.ins = "N"
ELSE
   mode.ins = "Y"
END IF
menus1.active = 0
CALL menudisplay

END SUB

SUB keyoff
'************************************************************************
'*  SUB:  keyofff                                                       *
'*                                                                      *
'*  Purpose:  Temporarily disable keytrapping during critical tasks.    *
'************************************************************************

KEY(1) OFF
KEY(2) OFF
KEY(11) OFF
KEY(12) OFF
KEY(13) OFF
KEY(14) OFF
KEY(15) OFF
KEY(16) OFF
KEY(17) OFF
KEY(18) OFF
KEY(19) OFF
KEY(20) OFF
KEY(21) OFF
KEY(22) OFF
KEY(23) OFF
KEY(24) OFF
KEY(25) OFF

END SUB

SUB keyon
'************************************************************************
'*  SUB:  keyon                                                         *
'*                                                                      *
'*  Purpose:  Enables keytrapping for certain keys defined in the main. *
'************************************************************************

KEY(1) ON
KEY(2) ON
KEY(11) ON
KEY(12) ON
KEY(13) ON
KEY(14) ON
KEY(15) ON
KEY(16) ON
KEY(17) ON
KEY(18) ON
KEY(19) ON
KEY(20) ON
KEY(21) ON
KEY(22) ON
KEY(23) ON
KEY(24) ON
KEY(25) ON

END SUB

SUB menudisplay
'*************************************************************************
'*  SUB:  menudisplay                                                    *
'*                                                                       *
'*  Purpose:  Displays the proper menu and highlights current selection. *                      *
'*************************************************************************

COLOR foreground, backround
IF menus1.active = 0 THEN                   'quit menus
   SCREEN vmode, , 0, 0                     'go back to original screen
   COLOR foreground, backround
   GOTO quitmenu
END IF

SCREEN vmode, , 1, 1                        'otherwise, get new screen
COLOR foreground, backround
CLS
LOCATE 1, 1
PRINT line1$;
LOCATE 2, 1
PRINT " File     Mode     Edit     Setup     Reset             Command Listing:";
LOCATE 3, 1
PRINT line1$;

SELECT CASE menus1.menunum                   'Highlight proper menu
   CASE 1
         LOCATE 2, 1
         COLOR backround, foreground
         PRINT " File ";
         COLOR foreground, backround
         LOCATE 3, 3
         PRINT line7$;
         LOCATE 4, 3
         PRINT line4$;
         LOCATE 5, 3
         PRINT line5$; line3$; " Open  ";
         LOCATE 6, 3
         PRINT line5$; line3$; " Save  ";
         LOCATE 7, 3
         PRINT line5$; line3$; " Print ";
         LOCATE 8, 3
         PRINT line6$; line3$; " Exit  ";
   CASE 2
         LOCATE 2, 10
         COLOR backround, foreground
         PRINT " Mode ";
         COLOR foreground, backround
         LOCATE 3, 12
         PRINT line7$;
         LOCATE 4, 12
         PRINT line4$;
         LOCATE 5, 12
         PRINT line5$; line3$; " Direct   ";
         LOCATE 6, 12
         PRINT line5$; line3$; " Record   ";
         LOCATE 7, 12
         PRINT line5$; line3$; " Playback ";
         LOCATE 8, 12
         PRINT line6$; line3$; " Edit     ";

   CASE 3
         LOCATE 2, 19
         COLOR backround, foreground
         PRINT " Edit ";
         COLOR foreground, backround
         LOCATE 3, 21
         PRINT line7$;
         LOCATE 4, 21
         PRINT line4$;
         LOCATE 5, 21
         PRINT line5$; line3$; " Cut    ";
         LOCATE 6, 21
         PRINT line5$; line3$; " Copy   ";
         LOCATE 7, 21
         PRINT line5$; line3$; " Paste  ";
         LOCATE 8, 21
         PRINT line6$; line3$; " Insert ";

   CASE 4
         LOCATE 2, 28
         COLOR backround, foreground
         PRINT " Setup ";
         COLOR foreground, backround
         LOCATE 3, 30
         PRINT line7$;
         LOCATE 4, 30
         PRINT line4$;
         LOCATE 5, 30
         PRINT line5$; line3$; " Delay   ";
         LOCATE 6, 30
         PRINT line5$; line3$; " Printer ";
         LOCATE 7, 30
         PRINT line6$; line3$; " Robot   ";

   CASE 5
         LOCATE 2, 37
         COLOR backround, foreground
         PRINT " Reset ";
         COLOR foreground, backround
         LOCATE 3, 39
         PRINT line7$;
         LOCATE 4, 39
         PRINT line4$;
         LOCATE 5, 39
         PRINT line5$; line3$; " Arm to 0's ";
         LOCATE 6, 39
         PRINT line5$; line3$; " Counters   ";
         LOCATE 7, 39
         PRINT line6$; line3$; " Clear Data ";
END SELECT

COLOR backround, foreground

SELECT CASE menus1.menunum         'Highlight proper menu selection
   CASE 1
      SELECT CASE menus1.sel
      CASE 1
         LOCATE 5, 5
         PRINT " Open  ";
         LOCATE 19, 12
         PRINT " Opens a command file (*.dat) ";
      CASE 2
         LOCATE 6, 5
         PRINT " Save  ";
         LOCATE 19, 12
         PRINT " Saves a command file (*.dat) ";
      CASE 3
         LOCATE 7, 5
         PRINT " Print ";
         LOCATE 19, 12
         PRINT " Prints a file (if you have a printer connected.) ";
      CASE 4
         LOCATE 8, 5
         PRINT " Exit  ";
         LOCATE 19, 12
         PRINT " Exits the ROBOT1 program. ";
      END SELECT
   CASE 2
      SELECT CASE menus1.sel
      CASE 1
         LOCATE 5, 14
         PRINT " Direct   ";
         LOCATE 19, 12
         PRINT " Direct control mode sends commands to the robot as you enter them. ";
      CASE 2
         LOCATE 6, 14
         PRINT " Record   ";
         LOCATE 19, 12
         PRINT " Records commands as you enter them. ";
      CASE 3
         LOCATE 7, 14
         PRINT " Playback ";
         LOCATE 19, 12
         PRINT " Will playback a series of commands that you have recored. ";
      CASE 4
         LOCATE 8, 14
         PRINT " Edit     ";
         LOCATE 19, 12
         PRINT " Allows you to edit the Command Listing. ";
      END SELECT
   CASE 3
      SELECT CASE menus1.sel
      CASE 1
         LOCATE 5, 23
         PRINT " Cut    ";
         LOCATE 19, 12
         PRINT " Copies the selected Command Listing to the clipboard. ";
         LOCATE 20, 12
         PRINT " Also deletes the commands from the Command Listing.   ";
      CASE 2
         LOCATE 6, 23
         PRINT " Copy   ";
         LOCATE 19, 12
         PRINT " Copies the selected Command Listing to the clipboard. ";
      CASE 3
         LOCATE 7, 23
         PRINT " Paste  ";
         LOCATE 19, 12
         PRINT " Inserts the contents of the clipboard into the Command Listing. ";
      CASE 4
         LOCATE 8, 23
         PRINT " Insert ";
         LOCATE 19, 12
         PRINT " Allows you to insert a blank command into the Command Listing. ";
      END SELECT
   CASE 4
      SELECT CASE menus1.sel
      CASE 1
         LOCATE 5, 32
         PRINT " Delay   ";
         LOCATE 19, 12
         PRINT " Sets the delay between commands to the robot. ";
      CASE 2
         LOCATE 6, 32
         PRINT " Printer ";
         LOCATE 19, 12
         PRINT " Sets the port number for the Printer. ";
      CASE 3
         LOCATE 7, 32
         PRINT " Robot   ";
         LOCATE 19, 12
         PRINT " Sets the port number for the Robot-arm. ";
      END SELECT
   CASE 5
      SELECT CASE menus1.sel
      CASE 1
         LOCATE 5, 41
         PRINT " Arm to 0's ";
         LOCATE 19, 12
         PRINT " Moves the robot-arm back to its original position. ";
      CASE 2
         LOCATE 6, 41
         PRINT " Counters   ";
         LOCATE 19, 12
         PRINT " Sets the counters to zeros.  Sets a new original position. ";
      CASE 3
         LOCATE 7, 41
         PRINT " Clear Data ";
         LOCATE 19, 12
         PRINT " Clears any data you may have recorded or loaded. ";
      END SELECT
END SELECT
COLOR foreground, backround

LOCATE 18, 1                         'Bottom of screen
PRINT line1$;
LOCATE 19, 1
PRINT "Messages:";
LOCATE 22, 1
PRINT line1$;
LOCATE 24, 1
PRINT line1$;

quitmenu:
LOCATE 21, 1
PRINT "Active Modes --->   Direct: "; mode.direct; "    Record: "; mode.record; "   Insert: "; mode.ins; "    Edit: ";
mode.edit;

LOCATE 23, 1
PRINT blank1$;
LOCATE 23, 1
PRINT "B: "; offset.b; "    G: "; offset.g; "    F: "; offset.f; "    R: "; offset.R; "    S: "; offset.s; "    W: ";
offset.w;

END SUB

SUB openit
'************************************************************************
'*  SUB:  openit                                                        *
'*                                                                      *
'*  Purpose:  Opens a command file and loads it into linked list.       *
'************************************************************************


COLOR backround, foreground
LOCATE 7, 25
PRINT "  Hit <SPACE BAR> to continue ";

LOCATE 8, 25
PRINT "   or hit <ESC> to cancel.    ";

CALL keyoff

a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN
   GOTO quitopenit
ELSE
   LOCATE 10, 25
   INPUT " Enter Filename (*.dat): ", filename$
END IF

OPEN filename$ FOR INPUT AS #1             'does file exist?
IF erroroccured = 1 THEN
   erroroccured = 0
   GOTO quitopenit
END IF

WHILE NOT EOF(1)                           'else load it

  INPUT #1, comm$
  mot1$ = MID$(comm$, 1, 1)
  dir1$ = MID$(comm$, 2, 1)
  dis1$ = MID$(comm$, 3, 3)
  IF INSTR("BGFRSW", mot1$) = 0 THEN         'legal characters?
     LOCATE 19, 12
     PRINT " There was an incorrect motor command in the file.  ";
     LOCATE 20, 12
     PRINT mot1$
     SLEEP
     CALL resetarray
     GOTO quitopenit
  END IF
  IF INSTR("UDLROC+-", dir1$) = 0 THEN
     LOCATE 19, 12
     PRINT " There was an incorrect direction command in the file.  "
     LOCATE 20, 12
     PRINT dir1$
     SLEEP
     CALL resetarray
     GOTO quitopenit
  END IF
  IF VAL(dis1$) < 0 OR VAL(dis1$) > 999 THEN
     LOCATE 19, 12
     PRINT " There was an incorrect distance command in the file.  "
     LOCATE 20, 12
     PRINT dis1$
     SLEEP
     CALL resetarray
     GOTO quitopenit
  END IF
  IF mode.ins = "N" THEN                     'if overtype mode
     CALL addlist(mot1$, dir1$, dis1$)
  ELSE                                       'if insert mode
     CALL inslist(mot1$, dir1$, dis1$)
  END IF

WEND
CLOSE #1
CALL displaylist

SCREEN vmode, , 1, 1                         'go back to menu screen
COLOR foreground, backround                  'reset color

quitopenit:
CLOSE #1
COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay                             'go back to main screen

END SUB

SUB paste
'************************************************************************
'*  SUB:  paste                                                         *
'*                                                                      *
'*  Purpose:  Inserts from the clipboard to the command listing.        *
'************************************************************************
IF mode.edit = "N" THEN GOTO quitpaste
oldmode$ = mode.ins
mode.ins = "Y"                          'insert mode works best
FOR i% = 1 TO clipcount                 'only works if something in clip
   CALL inslist(clip(i%).mot, clip(i%).dir, clip(i%).dis)
NEXT i%
mode.ins = oldmode$                     'reset original mode

CALL clearsel                           'clear all selections
CALL displaylist
quitpaste:
menus1.active = 0
CALL menudisplay

END SUB

SUB playbackmode
'************************************************************************
'*  SUB:  playbackmode                                                  *
'*                                                                      *
'*  Purpose:  Plays back a command sequence that has been loaded into   *
'*            memory.  Will do it forward or reverse.                   *
'************************************************************************

LOCATE 7, 24
PRINT line3$; line3$; line3$; line3$; line3$; line3$; line3$; line3$; line3$; line3$; line3$; line3$;
COLOR backround, foreground
LOCATE 7, 35
PRINT " Would you like: ";
LOCATE 8, 35
PRINT "                 ";
LOCATE 9, 35
PRINT "   F - Forward   ";
LOCATE 10, 35
PRINT "   R - Reverse   ";
LOCATE 11, 35
PRINT "                 ";
LOCATE 12, 35
PRINT " Enter F or R:   ";
COLOR foreground, backround

a$ = ""
DO WHILE a$ = ""
IF menus1.active = 0 THEN GOTO quitplayback
IF menus1.menunum <> 2 THEN GOTO quitplayback
IF menus1.sel <> 3 THEN GOTO quitplayback
a$ = INKEY$
LOOP

CALL keyoff

IF mode.arm = "N" THEN
   COLOR backround, foreground
   LOCATE 14, 10
   PRINT " Sorry, Can't do that since there is no robot hooked up.";
   LOCATE 15, 10
   PRINT "            Press any key to continue...                "
   COLOR foreground, backround
   DO WHILE INKEY$ = ""
   LOOP
   GOTO quitplayback
END IF

IF llist.head = llist.tail THEN
      COLOR backround, foreground
      LOCATE 14, 10
      PRINT " Sorry, there is nothing to playback. ";
      LOCATE 15, 10
      PRINT "      Press any key to continue...    "
      COLOR foreground, backround
      DO WHILE INKEY$ = ""
      LOOP
      menus1.active = 0
      GOTO quitplayback
END IF

IF a$ = "r" OR a$ = "R" THEN                       'reverse playback
   curr1% = llist.current
   DO
   curr1% = commarray(curr1%).last1
   comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
   CALL executeit(comm$)
   LOOP UNTIL (commarray(curr1%).last1 = 0 OR curr1% = llist.head)
ELSE
   curr1% = llist.head                             'else forward playback
   DO
   comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
   CALL executeit(comm$)
   curr1% = commarray(curr1%).next1
   LOOP UNTIL (commarray(curr1%).next1 = 0 OR curr1% = llist.tail)
   comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
   CALL executeit(comm$)
END IF

menus1.active = 0
quitplayback:
CALL menudisplay
CALL keyon
END SUB

SUB printerset
'************************************************************************
'*  SUB:  printerset                                                    *
'*                                                                      *
'*  Purpose:  Select the port that the printer is connected to.         *
'************************************************************************

COLOR backround, foreground
LOCATE 7, 45
PRINT " The printer status is:      ";
PRINT mode.printer;
LOCATE 8, 45
PRINT " On port:                "; mode.port1;
LOCATE 9, 45
PRINT " Enter a new status (Y or N): ";
LOCATE 10, 45
PRINT "   or hit <ESC> to cancel.    ";

CALL keyoff

a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN GOTO quitprintset

IF a$ = "Y" OR a$ = "y" THEN
   LOCATE 7, 45
   PRINT " Choose one of the following: ";
   LOCATE 8, 45
   PRINT "     1 - LPT1:                ";
   LOCATE 9, 45
   PRINT "     2 - LPT2:                ";
   LOCATE 10, 45
   PRINT "     <ESC> - to cancel        ";
   a$ = ""
   DO WHILE a$ = ""
   a$ = INKEY$
   LOOP
   IF a$ = CHR$(27) THEN GOTO quitprintset
   LOCATE 12, 45
   PRINT a$
   IF a$ = "2" THEN
       mode.port1 = "LPT2:"
       mode.port1add = &H278
       mode.printer = "Y"
   ELSE
       mode.port1 = "LPT1:"
       mode.port1add = &H378
       mode.printer = "Y"
   END IF
   OPEN "setup.inf" FOR OUTPUT AS #1

      PRINT #1, mode.printer
      PRINT #1, mode.port1
      PRINT #1, mode.arm
      PRINT #1, mode.port2
      PRINT #1, mode.delay
   CLOSE #1

ELSE
   IF a$ = "N" OR a$ = "n" THEN
      mode.printer = "N"
      mode.port1 = "     "
      OPEN "setup.inf" FOR OUTPUT AS #1
      PRINT #1, mode.printer
      PRINT #1, mode.port1
      PRINT #1, mode.arm
      PRINT #1, mode.port2
      PRINT #1, mode.delay
      CLOSE #1
   ELSE
      GOTO quitprintset
   END IF
END IF

quitprintset:
COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay

END SUB

SUB printit
'************************************************************************
'*  SUB:  printit                                                       *
'*                                                                      *
'*  Purpose:  Print a command listing if the printer is connected.      *
'************************************************************************


IF mode.printer = "N" THEN
   LOCATE 7, 25
   COLOR backround, foreground
   PRINT " Sorry, no printer connected. "
   SLEEP
   GOTO quitprintit
END IF

COLOR backround, foreground
LOCATE 7, 25
PRINT "  Hit <SPACE BAR> to continue ";

LOCATE 8, 25
PRINT "   or hit <ESC> to cancel.    ";

CALL keyoff

a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN
   GOTO quitprintit
ELSE
   LOCATE 10, 25
   INPUT " Enter Filename (*.dat): ", filename$
END IF

COLOR foreground, backround

OPEN filename$ FOR INPUT AS #1
IF erroroccured = 1 THEN
   erroroccured = 0
   GOTO quitprintit
END IF

OPEN mode.port1 FOR OUTPUT AS #2

PRINT #2, " ------- Listing of "; filename$; " --------"
PRINT #2, " "
WHILE NOT EOF(1)
  INPUT #1, comm$
  PRINT #2, comm$
WEND
PRINT #2, " ---------------  END OF FILE ------------"
CLOSE #1
CLOSE #2

quitprintit:

COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay

END SUB

SUB recordmode
'************************************************************************
'*  SUB:  recordmode                                                    *
'*                                                                      *
'*  Purpose:  Switch to and from RECORD MODE.                           *
'************************************************************************

IF mode.record = "Y" THEN
   mode.record = "N"
ELSE
   mode.record = "Y"
END IF
menus1.active = 0
CALL menudisplay

END SUB

SUB resetarm
'************************************************************************
'*  SUB:  resetarm                                                      *
'*                                                                      *
'*  Purpose:  Issues commands necessary to return arm to its original   *
'*            orientation.
'************************************************************************

CALL keyoff

IF mode.arm = "N" THEN
   COLOR backround, foreground
   LOCATE 13, 10
   PRINT " Sorry, Can't do that since there is no robot hooked up.";
   LOCATE 14, 10
   PRINT "            Press any key to continue...                "
   COLOR foreground, backround
   DO WHILE INKEY$ = ""
   LOOP
   GOTO quitresetarm
ELSE
   IF mode.direct = "N" THEN
      COLOR backround, foreground
      LOCATE 13, 10
      PRINT " Sorry, but you must be in DIRECT mode to do that.";
      LOCATE 14, 10
      PRINT "          Press any key to continue...            "
      COLOR foreground, backround
      DO WHILE INKEY$ = ""
      LOOP
      GOTO quitresetarm
  END IF
END IF

IF (offset.b = 0 AND offset.g = 0 AND offset.f = 0 AND offset.R = 0 AND offset.s = 0 AND offset.w = 0) THEN
   GOTO quitresetarm
END IF

IF offset.b > 0 THEN
  dist1$ = STR$(offset.b)
  comm$ = "B-" + LTRIM$(dist1$)
   CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.b))
   comm$ = "B+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF
IF offset.g > 0 THEN
   dist1$ = STR$(offset.g)
   comm$ = "G-" + LTRIM$(dist1$)
   CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.g))
   comm$ = "G+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF
IF offset.f > 0 THEN
   dist1$ = STR$(offset.f)
   comm$ = "F-" + LTRIM$(dist1$)
   CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.f))
   comm$ = "F+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF
IF offset.R > 0 THEN
   dist1$ = STR$(offset.R)
   comm$ = "R-" + LTRIM$(dist1$)
   CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.R))
   comm$ = "R+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF
IF offset.s > 0 THEN
   dist1$ = STR$(offset.s)
   comm$ = "S-" + LTRIM$(dist1$)
  CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.s))
   comm$ = "S+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF
IF offset.w > 0 THEN
   dist1$ = STR$(offset.w)
   comm$ = "W-" + LTRIM$(dist1$)
   CALL executeit(comm$)
ELSE
   dist1$ = STR$(-1 * (offset.w))
   comm$ = "W+" + LTRIM$(dist1$)
   CALL executeit(comm$)
END IF

quitresetarm:
menus1.active = 0
CALL menudisplay
CALL keyon
END SUB

SUB resetarray
'************************************************************************
'*  SUB:  resetarray                                                    *
'*                                                                      *
'*  Purpose:  Clears command array.                                     *
'************************************************************************

CALL keyoff
FOR i% = 1 TO arraymax                'init array (blank)
commarray(i%).mot = " "
commarray(i%).dir = " "
commarray(i%).dis = " "
commarray(i%).used = 0
commarray(i%).next1 = 0
commarray(i%).last1 = 0
NEXT i%

SCREEN vmode, , 0, 0                        'clear portions of screen
COLOR foreground, backround
FOR i = 1 TO 14
LOCATE i + 3, 64
PRINT "       "
NEXT i
SCREEN vmode, , 1, 1
COLOR foreground, backround
llist.head = 1                              'reset linked list counters
llist.tail = 1
llist.current = 1

menus1.active = 0
CALL menudisplay
CALL keyon
END SUB

SUB resetcount
'************************************************************************
'*  SUB:  resetcount                                                    *
'*                                                                      *
'*  Purpose:  Resets offset counters to zeros.                          *
'************************************************************************

offset.b = 0
offset.g = 0
offset.f = 0
offset.R = 0
offset.s = 0
offset.w = 0
menus1.active = 0
CALL menudisplay

END SUB

SUB robotset
'************************************************************************
'*  SUB:  robotset                                                      *
'*                                                                      *
'*  Purpose:  Sets which port the robot is connected to.                *
'************************************************************************

COLOR backround, foreground
LOCATE 7, 45
PRINT " The robot status is:        ";
PRINT mode.arm;
LOCATE 8, 45
PRINT " On port:                "; mode.port2;
LOCATE 9, 45
PRINT " Enter a new status (Y or N): ";
LOCATE 10, 45
PRINT "   or hit <ESC> to cancel.    ";

CALL keyoff

a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN GOTO quitrobotset

IF a$ = "Y" OR a$ = "y" THEN
   LOCATE 7, 45
   PRINT " Choose one of the following: ";
   LOCATE 8, 45
   PRINT "     1 - LPT1:                ";
   LOCATE 9, 45
   PRINT "     2 - LPT2:                ";
   LOCATE 10, 45
   PRINT "     <ESC> - to cancel        ";
   a$ = ""
   DO WHILE a$ = ""
   a$ = INKEY$
   LOOP
   IF a$ = CHR$(27) THEN GOTO quitrobotset
   LOCATE 12, 45
   PRINT a$
   IF a$ = "2" THEN
       mode.port2 = "LPT2:"
       mode.port2add = &H278
       mode.arm = "Y"
   ELSE
       mode.port2 = "LPT1:"
       mode.port2add = &H378
       mode.arm = "Y"
   END IF
   OPEN "setup.inf" FOR OUTPUT AS #1
      PRINT #1, mode.printer
      PRINT #1, mode.port1
      PRINT #1, mode.arm
      PRINT #1, mode.port2
      PRINT #1, mode.delay
   CLOSE #1

ELSE
   IF a$ = "N" OR a$ = "n" THEN
      mode.arm = "N"
      mode.port2 = "     "
      OPEN "setup.inf" FOR OUTPUT AS #1
      PRINT #1, mode.printer
      PRINT #1, mode.port1
      PRINT #1, mode.arm
      PRINT #1, mode.port2
      PRINT #1, mode.delay
      CLOSE #1
   ELSE
      GOTO quitrobotset
   END IF
END IF

quitrobotset:
COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay


END SUB

SUB saveit
'************************************************************************
'*  SUB:  saveit                                                        *
'*                                                                      *
'*  Purpose:  Saves a command listing to a file.                        *
'************************************************************************

IF llist.head = llist.tail THEN
   LOCATE 7, 25
   COLOR backround, foreground
   PRINT " Sorry, not enough to save. "
   SLEEP
   GOTO quitsaveit
END IF

COLOR backround, foreground
LOCATE 7, 25
PRINT "  Hit <SPACE BAR> to continue ";
LOCATE 8, 25
PRINT "   or hit <ESC> to cancel.    ";

CALL keyoff

a$ = ""
DO WHILE a$ = ""
a$ = INKEY$
LOOP
IF a$ = CHR$(27) THEN
   GOTO quitsaveit
ELSE
   LOCATE 10, 25
   INPUT " Enter Filename (*.dat): ", filename$
END IF

COLOR foreground, backround

OPEN filename$ FOR OUTPUT AS #1

curr1% = llist.head
comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
PRINT #1, comm$
DO
curr1% = commarray(curr1%).next1
comm$ = commarray(curr1%).mot + commarray(curr1%).dir + commarray(curr1%).dis
PRINT #1, comm$
LOOP UNTIL (commarray(curr1%).next1 = 0 OR curr1% = llist.tail)
CLOSE #1
quitsaveit:

COLOR foreground, backround
CALL keyon
menus1.active = 0
CALL menudisplay


END SUB

SUB scroll (direc%)
'************************************************************************
'*  SUB:  scroll (direction of scroll)                                  *
'*                                                                      *
'*  Purpose:  Adjusts the CURRENT node and calls displaylist.           *
'************************************************************************

IF direc% > 0 THEN
   IF commarray(llist.current).next1 <> 0 THEN
      sellist(llist.current) = 0
      llist.current = commarray(llist.current).next1
   END IF
ELSE
   IF commarray(llist.current).last1 <> 0 THEN
      sellist(llist.current) = 0
      llist.current = commarray(llist.current).last1
  END IF
END IF
sellist(llist.current) = 1
llist.look1 = llist.current
CALL displaylist
END SUB

SUB selection (dir%)
'************************************************************************
'*  SUB:  selection                                                     *
'*                                                                      *
'*  Purpose:  Keep track of selected entries in command listing.        *
'************************************************************************


IF llist.look1 = llist.head AND dir% = 0 AND llist.look2 = 1 GOTO quitsel
IF llist.current = llist.tail AND dir% = 1 AND llist.look2 = 0 GOTO quitsel
IF commarray(llist.current).used = 0 GOTO quitsel

IF llist.current = llist.look1 AND dir% = 0 THEN llist.look2 = 1   'if up first
IF llist.current = llist.look1 AND dir% = 1 THEN llist.look2 = 0   'if down first

IF dir% = 0 THEN                             'up has been pressed
    IF llist.look2 > 0 THEN                  'if going up from current
      temp% = commarray(llist.look1).last1
      llist.look1 = temp%
      sellist(temp%) = 1
    ELSE                                     'if going up from bottom
      temp% = commarray(llist.current).last1
      sellist(llist.current) = 0
      llist.current = temp%
    END IF
ELSE                                         'down has been pressed
    IF llist.look2 > 0 THEN                  'if going down from current
      temp% = commarray(llist.look1).next1
      sellist(llist.look1) = 0
      llist.look1 = temp%
    ELSE                                     'if going down from bottom
      temp% = commarray(llist.current).next1
      llist.current = temp%
      sellist(temp%) = 1
    END IF
END IF
CALL displaylist
quitsel:

END SUB