'-----[ HexHDATS_PLCD.bsp ]-----------------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... HexHDATS_PLCD.bsp
'   Purpose.... HexCrawler HDATS RC and/or LCD AppMod control.
'   Author..... CustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 3 April 2005
'   Updated....
'   Version.... 1.0
'
' Hardware
' (1) HexCrawler HDATS Robotic Kit
' (1) Parallax Board of Education
' (1) Parallax BS2p Module
' (2) Parallax Servo Controllers (PSC)
' (1) Parallax LCD AppMod
' (1) Radio controller Tx/Rx System (optional)
'     This program uses a Tower Hobbies 6 channel FM Tx
'     and 7 channel FM Rx.
'
'=========================================================================
'Getting Started
'=========================================================================
' 1. Physically Tune the HexCrawler HDATS
' 2. Run the RC_Test.bs2 program (optional - RC unit sold separatly)
' 3. Run the HexHDATS_PLCD.bsp Demo program
'
'=========================================================================
' First Time Use
'=========================================================================
' On power up the PSC will center all servos.  If you ran the
' HexHDATS_PLCD.bsp code after about 3 seconds the legs will move
' to their starting positions.
'
' As the robot initializes you should see the CrustCrawler banner and
' HexHDATS appear on the Parallax LCD Appmod. If not adjust the contrast
' POT!
'
' It is recommended that you hold the robot off the ground on the initial
' start up or until the leg have reached their starting positions.  Not
' doing so will cause unnecessary stress on the servo joints
'
' Use the LCD AppMod to:
' 1. Select Speed
' 2. Select Direction
' 3. Run selection
'
' If you are using a radio controller.
'
' 1. Select a speed using button B on the LCD AppMod
' 2. Power the transmitter and receiver
' 3. Press button D (run) on the LCD AppMod
' 4. Move the transmitter stick
'
' Please read the radio control section below for receiver to BOE
' connection and operation instructions.
'
'=========================================================================
' Parallax LCD AppMod Control
'=========================================================================
' LCD AppMod Buttons
' A ...... Stop/Enter selection mode
' B ...... Select speed (0-3)
' C ...... Select direction (0-7)
' D ...... Run (enter selection)
'
' BUTTON A
' Press and hold button A until the robot stops.  The LCD will the
' show current speed and direction settings.
'
' ----------
' |spd  Dir|
' |0    00 |
' ----------
' A  B  C  D
'
' Button B
' Press button B to change speeds from 0 to 15
' 0 ....... Fastest
' 1 ....... Default
' 3 ....... Slowest
'
' Button C
' Button C controls direction from 0 to 7 where 0 can be considered
' north.  For example, if you positioned the robot to physically face
' north and the direction is set to 0 the robot will walk forward
' or north.  A direction of 01 is northeast and 07 is northwest.
'
' Button D
' Press button D to accept/run the selected speed and direction settings.
'
'=========================================================================
' LCD AppMod display
'=========================================================================
' The LCD displays the current speed and direction settings in
' selection mode.
' ----------
' |spd  Dir|
' |0    00 |
' ----------
' A  B  C  D
'
' The LCD shows the current gait setting on line 2 and "LCD Mode" on line 1 while
' the robot is walking.
' ----------
' |LCD Mode|
' |0    00 |
' ----------
' A  B  C  D
'
' The LCD shows the current gait setting on line 2 and "RC Mode" on line 1 while
' the robot is walking under RC control.
' ----------
' |RC Mode |
' |0    00 |
' ----------
' A  B  C  D
'
'=========================================================================
' Radio Control
'=========================================================================
' The Hex HDATS is RC ready.  Simply Plug your RC receiver into the
' BS2p and away you go.  The instructions below are for connecting a
' Tower Hobbies 6 channel radio control system to the BOE.  Your
' receiver channels might differ.  Please refer to your receiver's
' instruction manual.
'
' We suggest running the RC_Test.bs2 program before running the
' Hex HDATS in radio control mode.  The Nomad_RC_Test.bs2 program
' verifies that the receiver is connected correctly to the BOE.
' http://www.crustcrawler.com/products/hexcrawler_hdats.php?id=4

' Connections
' You will need 4 wires to connect the receiver to the BOE's X5
' servo port (see your BOE Rev B documentation).
' Soldering 8 High Density Female RS232 Sockets to the ends
' of four 20 gage wires works great.
'
' Connect the receiver's
' 1. Throttle signal line (ch3)to X5 Pin 12 (outside pin) on the BOE.
' 2. Throttle ground (ch3)to X5 Pin 12 (inside pin) on the BOE
' 3. Rudder signal line (ch4) to X5 Pin 13 (outside pin)
' 4. Rudder ground (ch4) to X5 Pin 13 (inside pin) on the BOE
'
'=========================================================================
' BOE X5 and X4 servo ports
'=========================================================================
'            X5       X4
'        |--------|--------|  |------|
' Ground |  c  c  |  x  x  |  | 5V   |----
' N/C    |  x  x  |  x  x  |  | Reg  |----
' Signal |  c  c  |  x  x  |  |      |----
'        |--------|--------|  |------|
'           12 13    14 15
'
' The Demo program is designed to detect the presence of a receiver
' on Pins 14 and 15.  Once detected simply move the transmitter stick.
' The to Hex HDATS should respond.  If not check your connections.
'
'---- [Programming Notes] ------------------------------------------------
'
'
'
'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15             ' PSC module
Baud            CON     1021+$8000     ' 2400 baud
StickXPin       PIN     13             ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12             ' Up/Down(Y) joystick Rx Ch3

' -----[ LCD I/O Definitions ]--------------------------------------------
E               PIN     1              ' LCD Enable (1 = enabled)
RW              PIN     2              ' Read/Write
RS              PIN     3              ' Reg Select (1 = char)
LcdDirs         VAR     DIRB           ' dirs for I/O redirection
LcdBusOut       VAR     OUTB
LcdBusIn        VAR     INB

LcdCls          CON     $01            ' clear the LCD
LcdHome         CON     $02            ' move cursor home
LcdCrsrL        CON     $10            ' move cursor left
LcdCrsrR        CON     $14            ' move cursor right
LcdDispL        CON     $18            ' shift chars left
LcdDispR        CON     $1C            ' shift chars right

LcdDDRam        CON     $80            ' Display Data RAM control
LcdCGRam        CON     $40            ' Character Generator RAM
LcdLine1        CON     $80            ' DDRAM address of line 1
LcdLine2        CON     $C0            ' DDRAM address of line 2

LcdScrollTm     CON     250            ' LCD scroll timing (ms)

'-----[ Leg Constants ]---------------------------------------------------
Center          CON     750
LeftDown        CON     300
RightDown       CON     1200
LiftOffset      CON     400

'----[ Servo Speed Constants ]-------------------------------------------
' Adjustable horizontal ramp (servo speed)
VeryFast        CON     $7           ' Horizontal speed
Default         CON     $8
Slow            CON     $9
VerySlow        CON     $A
speeds          CON     $4           ' Number of speeds settings (4)
directions      CON     $8           ' Number fo directions (8)
' Adjustable Vertical ramp (servo speed)
VRamp           CON     $7           ' Lift speed

'---- [ Horizontal Offset From Center ]-----------------------------------
'Every 5 units = 1.8 degrees from center
HorzOffset      CON     75
'HorzOffset      CON     30

'-----[ Horizontal Table Constants ]--------------------------------------
'Columns           CON      3
HT_Rows         CON      2             ' Horizontal table rows
'HT_Rows         CON      3             ' Horizontal table rows
HT_Row          CON      3             ' Number of columns

'-----[ Vertical Table Constants ] ---------------------------------------
VT_Row          CON      2              ' Vertical table columns

'-----[ Put and Get Stack ]-----------------------------------------------
PassValue       CON      0              ' Stack Position
PassPointer     CON      4              ' Stack Position

'-----[ Walking Variables ]-----------------------------------------------
stickXPos       VAR     Word            ' Left/Right (X) joystick posn
stickYPos       VAR     Word            ' Up/Down(Y) joystick position
ptrEEPROM       VAR     Word            ' Gait select
temp            VAR     Word            ' Temp variable
eepromData      VAR     Byte(3)         ' Servo Position
servoAddr       VAR     Byte            ' Servo addresses
ramp            VAR     Byte            ' Rotation velocity
gaitCode        VAR     Byte            ' Gait Code
hOffset         VAR     Byte            ' Angle legs
delay           VAR     Byte            ' time delay
char            VAR     Byte            ' character sent to LCD
scan            VAR     Byte            ' loop counter
ctrlNib         VAR     Nib             ' Flags
dirBit          VAR     ctrlNib.BIT0    ' Change directions
crabBit         VAR     ctrlNib.BIT1    ' 1 = crabwalk
angleDirBit     VAR     ctrlNib.BIT2    ' angle horz direction
liftBit         VAR     ctrlNib.BIT3    ' lift and lower
idx             VAR     Nib             ' loop counter

buttons         VAR     Nib             ' Buttons
btnA            VAR     buttons.BIT0    ' left-most button
btnB            VAR     buttons.BIT1
btnC            VAR     buttons.BIT2
btnD            VAR     buttons.BIT3    ' right-most

'---- [EEPROM Data] ------------------------------------------------------
'BoHT DATA  Word 680,  10,
           'Word 770,  0
'EoHT DATA  Word 820,  10

BoHT DATA  Word 680,  0
EoHT DATA  Word 820,  0

BoCHT DATA 2, Word 820,
           2, Word 750
EoCHT DATA 2, Word 680


BoLLT DATA Word RightDown,
           Word RightDown,
           Word LeftDown,
           Word RightDown-LiftOffset,
           Word LeftDown+LiftOffset,
           Word LeftDown+LiftOffset

BoLRT DATA Word RightDown,
           Word LeftDown,
           Word LeftDown,
           Word RightDown-LiftOffset,
           Word RightDown-LiftOffset,
           Word LeftDown+LiftOffset

Msg1       DATA  "Crust",0
Msg2       DATA  " Crawler",0
Msg3       DATA  "Robotics",0
Msg4       DATA  "Presents"
Msg5       DATA  "The",0
Msg6       DATA  "HexHDATS",0
Msg7       DATA  "LCD Mode",0
Msg8       DATA  "RC Mode ",0
Nav        DATA  "Spd  Dir",0

Initialize:
  ctrlNib = %0000                      ' Tripod forward
  hOffset = 0                          ' 0 = horizontal center
  gaitCode = $10

  GOSUB Initialize_LCD                 ' Init LCD
  GOSUB CrustCrawler_Logo              ' Write CC Logo
  char = LcdCls                        ' Clear Screen
  GOSUB Write_LCD_Command
  GOSUB LCD_Write_Nav                  ' Write navigation string
  GOSUB LCD_Write_Speed                ' Write Speed
  GOSUB LCD_Write_Direction            ' Write Direction
  ptrEEPROM = BoHT
  char = $FF

Main:
' Loop through legs 1, 3, and 5
' then through 2, 4, and 6
  FOR servoAddr = 0 TO 8 STEP 4
    IF crabBit = 0 THEN
      GOSUB Tri_Positions
    ELSE
      GOSUB Crab_Positions
    ENDIF
  GOSUB Write_PSC
  NEXT
PAUSE delay*10
  FOR servoAddr = 2 TO 10 STEP 4
    IF crabBit = 0 THEN
      GOSUB Tri_Positions
    ELSE
      GOSUB Crab_Positions
    ENDIF
  GOSUB Write_PSC
  NEXT
PAUSE delay*10
' Setup EEPROM pointer
  IF crabBit = 0 THEN
    IF (ptrEEPROM = BoHT) OR (ptrEEPROM = EoHT) THEN
      liftBit = liftBit + 1
      GOSUB Lift_Lower
    ENDIF
    IF liftBit = 0 THEN
      ptrEEPROM = ptrEEPROM - HT_Row
    ELSE
      ptrEEPROM = ptrEEPROM + HT_Row
    ENDIF
  ELSE
    IF (ptrEEPROM = EoCHT) THEN
      ptrEEPROM = BoCHT
    ENDIF
    GOSUB Lift_Lower
    ptrEEPROM = ptrEEPROM + HT_Row
  ENDIF
 'DEBUG CLS, ?ptrEEPROM, ?Tune, CR
 'GOTO Main
'-----[ Decisions ]-------------------------------------------------------
Check_First_Run:                       ' First run
  IF char = $FF THEN
    char = $00
    'DEBUG CLS, "Enter Select Speed and Direction"
    'PAUSE 2000
    GOTO Select_Speed_Direction
  ENDIF

Check_ButtonA_Press:                   ' Check for button a press
  GOSUB LCD_Get_Buttons
  IF btnA = 1 THEN
    GOTO Select_Speed_Direction
  ENDIF

Check_RC_Power:                        ' Check for Rx signal
  GOSUB Get_Stick
  IF stickXPos = 0  THEN
    'GOTO Check_RC_Power               ' Un-comment for RC only mode
    GOSUB LCD_Write_Walking            ' Write "Walking" on LCD
    GOTO Main
  ELSE
    GOTO Get_RC_Direction              ' Convert Rx data to gaitCode
  ENDIF

GOTO Main
END

'----[ TriPod Subs ] -----------------------------------------------------
Tri_Positions:
  SELECT servoAddr
    CASE 2,8
      READ (((HT_Rows-1)*HT_Row)-ptrEEPROM), eepromData(0),
            eepromData(1), eepromData(2)
    CASE ELSE
      READ ptrEEPROM, eepromData(0),
           eepromData(1), eepromData(2)
  ENDSELECT

  IF servoAddr < 7 THEN
    temp = Center - eepromData(2)
  ELSE
    temp = Center + eepromData(2)
  ENDIF
RETURN

'----[ Crab Subs ] -------------------------------------------------------
Crab_Positions:
  READ ptrEEPROM, eepromData(0),
       eepromData(1), eepromData(2)
  PUT 0, eepromData(1),eepromData(2)
  IF angleDirBit = 0 THEN
    temp = center - (eepromData(0)*hOffset)
  ELSE
    temp = center + (eepromData(0)*hOffset)
  ENDIF
  eepromData(0) = temp.LOWBYTE
  eepromData(1) = temp.HIGHBYTE
  GET 0, temp.LOWBYTE, temp.HIGHBYTE

  SELECT servoAddr
    CASE 0,4,8
      temp = 2*Center - temp
  ENDSELECT

RETURN

'-----[ Write to the PSC ]------------------------------------------------
Write_PSC:
PUT 20, ramp
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   eepromData(0), eepromData(1), CR]
  IF crabBit = 0 THEN
    ramp = Ramp+$6
  ENDIF
  SEROUT PSC,Baud,["!SC",((servoAddr/2)+16), ramp, temp.LOWBYTE,
                   temp.HIGHBYTE, CR]
GET 20, ramp
RETURN

'-------------------------------------------------------------------------
Lift_Lower:
  PUT PassPointer, Word ptrEEPROM
' Position EEPROM pointer
  IF dirBit = 0 THEN
    ptrEEPROM = BoLLT
    FOR servoAddr = 1 TO 9 STEP 4
      GOSUB Vertical_Servos
    NEXT
    FOR servoAddr = 3 TO 11 STEP 4
      GOSUB Vertical_Servos
    NEXT
  ELSE
    ptrEEPROM = BoLRT
    FOR servoAddr = 3 TO 11 STEP 4
      GOSUB Vertical_Servos
    NEXT
    FOR servoAddr = 1 TO 9 STEP 4
      GOSUB Vertical_Servos
    NEXT
  ENDIF
  dirBit = dirBit + 1
  GET PassPointer, Word ptrEEPROM
RETURN

Vertical_Servos:
  READ ptrEEPROM, temp.LOWBYTE, temp.HIGHBYTE
  SEROUT PSC,Baud,["!SC",servoAddr, VRamp, temp.LOWBYTE, temp.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + VT_Row
RETURN

'-----[ Parse GaitCode ]--------------------------------------------------
Parse_gaitCode:
'DEBUG CLS, "Top of Parse_gaitCode:", CR, HEX ?ctrlNib, HEX ?gaitCode
'PAUSE 1000
  SELECT gaitCode.LOWNIB
    CASE $0                            'Tripod Forward
      ctrlNib = %0000
      hOffset = 0
    CASE $1                            ' CrabRight Forward
      ctrlNib = %0010
      hOffset = HorzOffset
    CASE $2                            ' CrabRight
      ctrlNib = %0010
      hOffset = 0
    CASE $3                             ' CrabRight Reverse
      ctrlNib = %0110
      hOffset = HorzOffset
    CASE $4                             ' Reverse
      ctrlNib = %0001
      hOffset = 0
    CASE $5                             ' CrabLeft Reverse
      ctrlNib = %0111
      hOffset = HorzOffset
    CASE $6                             ' CrabLeft
      ctrlNib = %0011
      hOffset = 0
    CASE $7                             ' CrabLeft Forward
      ctrlNib = %0011
      hOffset = HorzOffset
    CASE ELSE
      ctrlNib = %0000
      hOffset = 0
    ENDSELECT
  SELECT gaitCode.HIGHNIB               ' Robot speed
    CASE $0
      ramp = VeryFast
      delay = 0
    CASE $1
      ramp = Default
      delay = 3
    CASE $2
      ramp = Slow
      delay = 10
    CASE $3
      ramp = VerySlow
      delay = 20
    CASE ELSE
      ramp = Default
      delay = 3
  ENDSELECT
'DEBUG CLS, "Bottom of Parse_gaitCode:", CR, HEX ?ctrlNib, HEX ?gaitCode
'PAUSE 1000
Set_Pointer:
  IF crabBit = 0 THEN
    ptrEEPROM = BoHT                    ' set pointer
  ELSE
    ptrEEPROM = BoCHT
  ENDIF
  GOSUB Lift_Lower
'DEBUG CLS, "After Lift Routine:", CR, HEX ?ctrlNib, HEX ?gaitCode
'PAUSE 1000
GOTO Main

'-----[ LCD Subroutines ]-------------------------------------------------
Select_Speed_Direction:                ' Select speed and direction
  GOSUB LCD_Write_Nav
  DO WHILE btnD = 0                    ' Do while D not pressed
    GOSUB LCD_Get_Buttons
    IF btnB = 1 THEN                   ' Speed setting
      gaitCode.HIGHNIB = (gaitCode.HIGHNIB+1)//speeds
      GOSUB LCD_Write_Speed            ' Write speed to LCD
    ENDIF
    IF btnC = 1 THEN                   ' Direction setting
      gaitCode.LOWNIB = (gaitCode.LOWNIB+1)//directions
      GOSUB LCD_Write_Direction        ' Write direction to LCD
    ENDIF
     'DEBUG CLS, "Select Speed and Direction:", CR, HEX ?ctrlNib, HEX ?gaitCode
  LOOP
  GOSUB LCD_Write_Walking              'Write "Walking"
GOTO Parse_gaitCode

  LCD_Get_Buttons:
    LcdDirs = %0000                    ' make LCD bus inputs
    buttons = %1111                    ' assume all pressed
    FOR scan = 1 TO 10
      buttons = buttons & LcdBusIn     ' make sure button held
      PAUSE 7                          ' debounce 10 x 5 ms
    NEXT
    LcdDirs = %1111                    ' return bus to outputs
  RETURN

LCD_Put_String:
  DO
    READ ptrEEPROM, char               ' Read data at EEPROM address
    IF (char = 0) THEN EXIT            ' 0 = End of EEPROM String
    GOSUB Write_LCD_Char               ' Write character to LCD
    ptrEEPROM = ptrEEPROM + 1          ' Increment EEPROM pointer
  LOOP
  RETURN

Write_LCD_Command:                     ' Low RS = LCD directive like
    LCDCMD E, char
  RETURN

  Write_LCD_Char:                      ' HIGH RS = Write a character
    LCDOUT E, 0, [char]
  RETURN

LCD_Write_Speed:
  PUT PassPointer, Word ptrEEPROM
  char = LcdLine2+0                    ' Write speed to LCD
  GOSUB Write_LCD_Command
  temp = gaitCode.HIGHNIB/10           ' Convert to ASCII
  char = temp+48
  GOSUB Write_LCD_Char

  char = LcdLine2+1
  GOSUB Write_LCD_Command

  temp = gaitCode.HIGHNIB//10
  char = temp+48
  GOSUB Write_LCD_Char
  GET PassPointer, Word ptrEEPROM
RETURN

LCD_Write_Direction:                   ' Write Direction to LCD
PUT PassPointer, Word ptrEEPROM
    char = LcdLine2+5
    GOSUB Write_LCD_Command
    temp = gaitCode.LOWNIB/10          ' Convert to ASCII
    char = temp+48
    GOSUB Write_LCD_Char

    char = LcdLine2+6
    GOSUB Write_LCD_Command

    temp = gaitCode.LOWNIB//10
    char = temp+48
    GOSUB Write_LCD_Char
GET PassPointer, Word ptrEEPROM
RETURN

LCD_Write_Nav:
PUT PassPointer, Word ptrEEPROM
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Nav                      ' Point to Nav
  GOSUB LCD_Put_String                 ' Write string
GET PassPointer, Word ptrEEPROM
RETURN

LCD_Write_Walking:
PUT PassPointer, Word ptrEEPROM
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Msg7                     ' Point to message 7
  GOSUB LCD_Put_String                 ' Write string
GET PassPointer, Word ptrEEPROM
RETURN

LCD_Write_RC_Mode:
PUT PassPointer, Word ptrEEPROM
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Msg8                     ' Point to message 8
  GOSUB LCD_Put_String                 ' Write string
GET PassPointer, Word ptrEEPROM
RETURN

CrustCrawler_Logo:                     ' Dispaly CC logo
    ptrEEPROM = Msg1                   ' Point to message 1
    GOSUB LCD_Put_String               ' Write string
    char = LcdLine2+0                  ' Go to Line 2
    GOSUB Write_LCD_Command            ' Write command
    ptrEEPROM = Msg2                   ' Point to message 2
    GOSUB LCD_Put_String               ' Write string
    PAUSE 2000                         ' wait 2 secs
    char = LcdCls
    GOSUB Write_LCD_Command
    ptrEEPROM = Msg5                   ' Point to message 5
    GOSUB LCD_Put_String               ' Write string
    char = LcdLine2+0                  ' Go to Line 2
    GOSUB Write_LCD_Command            ' Write command
    ptrEEPROM = Msg6                   ' Point to message 6
    GOSUB LCD_Put_String               ' Write string
    PAUSE 2000
  RETURN

Initialize_LCD:
  LCDCMD E, %00110000 : PAUSE 5        ' 8-bit mode
  LCDCMD E, %00110000 : PAUSE 0
  LCDCMD E, %00110000 : PAUSE 0
  LCDCMD E, %00100000 : PAUSE 0        ' 4-bit mode
  LCDCMD E, %00101000 : PAUSE 0        ' 2-line mode
  LCDCMD E, %00001100 : PAUSE 0        ' no crsr, no blink
  LCDCMD E, %00000110                  ' inc crsr, no disp shift
  char = LcdCls                        ' Clear screen
  GOSUB Write_LCD_Command
RETURN

'-----[ RC ]--------------------------------------------------------------
Get_RC_Direction:
PUT 10, gaitCode.LOWNIB
GOSUB LCD_Write_RC_Mode                ' Write "RC Mode" to LCD
GOSUB Get_Stick
  SELECT stickYpos                     ' Convert to gaitCode
    CASE < 1800
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $1
        CASE < 1800
          gaitCode.LOWNIB = $7
        CASE ELSE
          gaitCode.LOWNIB = $0
      ENDSELECT
    CASE < 2200
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $2
        CASE < 1800
          gaitCode.LOWNIB = $6
        CASE ELSE
          gaitCode.LOWNIB = $F
       ENDSELECT
    CASE < 2600
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $3
        CASE < 1800
          gaitCode.LOWNIB = $5
        CASE ELSE
          gaitCode.LOWNIB = $4
       ENDSELECT
    CASE ELSE
      gaitCode.LOWNIB = $F
  ENDSELECT

  IF (gaitCode.LOWNIB = $F) THEN
    IF StickXPos = 0 THEN
      GOTO Parse_gaitCode
    ELSE
      GOTO Get_RC_Direction
    ENDIF
  ENDIF

 GET 10, temp
  IF gaitCode.LOWNIB = temp.LOWNIB THEN
    GOTO Main
  ENDIF

  IF StickXPos = 0 THEN
    gaitCode = $10
    GOTO Parse_gaitCode
  ENDIF
  GOTO Parse_gaitCode


Get_Stick:
  PULSIN StickXPin, 1, stickXPos       ' Read joystick positions
  PULSIN StickYPin, 1, stickYPos       ' from transmitter P13, P12
RETURN

'----[ Debug Routines ]---------------------------------------------------
'Display2:
  'DEBUG CLS, DEC ?servoAddr, DEC ?verticalPosition, ?raiseBit
  'PAUSE 1000
'RETURN

'Display:
  'temp.LOWBYTE = servoPosition(0)
 ' temp.HIGHBYTE = servoPosition(1)
  'DEBUG CLS, ?ptrEEPROM, ?servoAddr, ?temp, ?anklePosition
  'PAUSE 500
  'END
'RETURN