idx             VAR     Nib             ' digit index
btnIndx         VAR     Nib             ' current digit to display

'---- [Button states] ----------------------------------------------------
Pressed         CON     1               ' Button states
NotPressed      CON     0

'---- [Walking Variables] ------------------------------------------------
servoAddr       VAR     Byte            ' Servo addresses
gaitCode        VAR     Byte            ' Current mode
ramp            VAR     Byte            ' Ramp used in SEROUT
rightRamp       VAR     Byte            ' Right side ramp values
leftRamp        VAR     Byte            ' Left side ramp values
ptrEEPROM       VAR     Word            ' Gait select
servoPosition   VAR     Word            ' Servo Position
stickXPos       VAR     Word            ' Left/Right (X) joystick posn
stickYPos       VAR     Word            ' Up/Down(Y) joystick position

'----- [Adjustable Ramp Values] ------------------------------------------
'Off  fast to slow
' 0    $1  -- $3F(63)
LiftRamp        CON     $1              ' Vertical servo ramp
VeryFast        CON     $8              ' Walk very fast
Fast            CON     $A              ' Walk fast

'-----[ Horizontal Leg Constants ]----------------------------------------
'Servo center
Center1         CON     750            ' Leg1 horizontal servo
Center2         CON     750            ' Leg2 horizontal servo
Center3         CON     750            ' Leg3 horizontal servo
Center4         CON     750            ' Leg4 horizontal servo

'-----[ Stride Calculations ]---------------------------------------------
Stride          CON     150             ' Stride units
delay           CON     Stride/5        ' Delay as a ratio of stride

Leg1Center      CON     Center1
Leg1Forward     CON     Center1+Stride  ' Stride + Leg1 Center
Leg1Back        CON     Center1-Stride

Leg2Center      CON     Center2
Leg2Forward     CON     Center2+Stride
Leg2Back        CON     Center2-Stride

Leg3Center      CON     Center3
Leg3Forward     CON     Center3-Stride
Leg3Back        CON     Center3+Stride

Leg4Center      CON     Center4
Leg4Forward     CON     Center4-Stride
Leg4Back        CON     Center4+Stride

'----- [Adjustable vertical servo positions] -----------------------------
RaiseRight      CON     300             ' Raised and lowered
LowerRight      CON     1200            ' vertical servo values
RaiseLeft       CON     1200
LowerLeft       CON     300

'---- [7 Segment LED and gaitCode EEPROM Data] -----
' Hex #    0   1   2   3   4   5   6   7   8   9   A   B   C   D   E
LED  DATA $7E,$18,$6D,$3D,$1B,$37,$77,$1C,$7F,$1F,$5F,$73,$66,$79,$67
Gait DATA $00,$01,$02,$10,$11,$12,$20,$21,$22,$30,$31,$32,$40,$41,$42

'---- [Servo Address and Position EEPROM data] -----------------
' Walk Forward
Forward DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF ' end of forward

' Walk Backward
Back    DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF 'end of back

' Left Turn EEPROM values
LTurn   DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward+Stride, $06,Word Leg4Back+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Center+Stride,  $06,Word Leg4Center+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF ' end of left turn

' Right turn EEPROM Values
RTurn   DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Center-Stride,
             $04,Word Leg3Center-Stride,  $06,Word Leg4Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back-Stride,
             $04,Word Leg3Forward-Stride, $06,Word Leg4Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF  ' end of right turn

'                Raise                Center           Lower
Adjust  DATA $01,Word RaiseRight, $00,Word Center1, $01,Word LowerRight
        DATA $03,Word RaiseRight, $02,Word Center2, $03,Word LowerRight
        DATA $05,Word RaiseLeft,  $04,Word Center3, $05,Word LowerLeft
        DATA $07,Word RaiseLeft,  $06,Word Center4, $07,Word LowerLeft,
        $FF ' end of adjust legs
'---- [End EEPROM Data] -----------------

'---- [Initialize] -----------------
DIRL = %01111111                        ' P0 - P6 are outputs
gaitCode = $10                          ' Walk straight forward fast
btnIndx = $03                           ' Init button index
segments = $3D                          ' Display a 3 on LED
GOTO Get_GaitCode                       ' Select a gait code

'---- [Gait Selection] ---------------------------------------------------
Parse_GaitCode:                         ' Assign gait parameters

IF gaitCode = $03 THEN
  GOTO Get_Stick
ENDIF

  SELECT gaitCode
    CASE $00                            ' Adjust Legs
      ptrEEPROM = Adjust
      rightRamp = Fast
      leftRamp = Fast
      GOTO Walking_Engine
    CASE $01
      ptrEEPROM = LTurn                 ' Spin left
      rightRamp = VeryFast              ' Assign ramp to
      leftRamp = VeryFast               ' left and right sides
      GOTO Walking_Engine
    CASE $02
      ptrEEPROM = RTurn                 ' Spin right
      rightRamp = VeryFast
      leftRamp = VeryFast
      GOTO Walking_Engine
  ENDSELECT

' HIGHNIB of gaitCode = direction
' Assign EEPROM pointer
  IF gaitCode.HIGHNIB <= $2 THEN
    ptrEEPROM = Forward
  ELSE
    ptrEEPROM = Back
  ENDIF

Setup_Ramp_Vars:
' HIGHNIB of gaitCode = Speed
' Assign ramp (servo speed)
  IF (gaitCode.HIGHNIB = $1) OR (gaitCode.HIGHNIB = $4) THEN
    ramp = VeryFast
  ELSE
    ramp = Fast
  ENDIF

' Check LowNib to determine
' left and right leg ramps
' for gradual turns
' 0 = straight
' 1 = slow left side
' 2 = slow right side
  SELECT gaitCode.LOWNIB
    CASE $0
      rightRamp = ramp
      leftRamp = rightRamp
    CASE $1
      rightRamp = ramp
      leftRamp = (ramp+$2)
    CASE $2
      leftRamp = ramp
      rightRamp = (ramp+$2)
  ENDSELECT

'-----[ Main Walking Routine ]--------------------------------------------
Walking_Engine:
READ ptrEEPROM,ServoAddr
DO WHILE servoAddr <> $FF
  IF (servoAddr // 2) = 1 THEN
    ramp = LiftRamp
  ELSE
    IF (servoAddr = $00) OR (servoAddr = $02) THEN
      ramp = rightRamp
    ENDIF
    IF(servoAddr = $04) OR (servoAddr = $06) THEN
      ramp = leftRamp
    ENDIF
  ENDIF
  GOSUB Write_PSC
LOOP
' If adjustment mode end
  IF ptrEEPROM = Adjust + 36 THEN         ' Adjust legs selected
    END
  ENDIF
'Check for a button press
  GOSUB Read_Buttons
  IF (btns.BIT0 = Pressed) OR (btns.BIT1 = Pressed) THEN
    GOTO Get_GaitCode                    ' Select gait code
  ELSE
    GOTO Get_Stick
  ENDIF
Continue:
  ptrEEPROM = ptrEEPROM - 48           ' Reset EEPROM pointer
  GOTO Walking_Engine                  ' Continue walking

'----- [Serial Out EEPROM values to PSC] ---------------------------------
Write_PSC:
  READ ptrEEPROM+1, servoPosition.LOWBYTE,
       servoPosition.HIGHBYTE
  SEROUT PSC,Baud,["!SC",ServoAddr,
         Ramp,servoPosition.LOWBYTE,
         servoPosition.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + 3
  READ ptrEEPROM,servoAddr
  PAUSE delay
RETURN

'---- [Button Code Section] ------
Get_GaitCode:
  DO WHILE (btns.BIT0 = NotPressed) OR (btns.BIT1 = NotPressed)
    IF (btns > %00) AND (btns < %11) THEN      ' one or the other pressed?
      btnIndx = btnIndx + btn1 // 15           ' increment if Button1 = 1
      btnIndx = btnIndx + (14 * btn2) // 15    ' decrement if Button2 = 1
      READ btnIndx, segments                   ' Update LED display
      READ gait + btnIndx, gaitCode            ' Update Selected gait Mode
      PAUSE 250                                ' 1/4 sec between changes
    ENDIF
    GOSUB Read_Buttons
  LOOP
  GOTO Parse_GaitCode

'----- [Read Buttons] ---------------------------------------------------
Read_Buttons:
  btns = %0011                                 ' assume both pressed
  FOR idx = 1 TO 5
    btns.BIT0 = btns.BIT0 & ModeBtn            ' scan mode button
    btns.BIT1 = btns.BIT1 & StartBtn           ' scan start/stop button
    PAUSE 5                                    ' debounce delay
  NEXT
RETURN

' Convert Stick positions to gaitcode
Get_Stick:
PULSIN StickXPin, 1, stickXPos                 ' Read joystick positions
PULSIN StickYPin, 1, stickYPos                 ' from transmitter P13, P12

IF (stickXPos & stickYPos) < 500 THEN          ' Is the RC Tx off
  GOTO Continue                                ' Continue walking
ELSE
  SELECT stickYPos
    CASE 580 TO 650
      gaitCode = $10
    CASE 651 TO 730
      gaitCode = $20
    CASE 731 TO 800
      gaitCode = $00
    CASE 801 TO 870
      gaitCode = $30
    CASE 871 TO 960
      gaitCode = $40
  ENDSELECT
  SELECT stickXPos
    CASE 500 TO 700
      gaitCode = gaitCode | $01
    CASE 701 TO 820
      gaitCode = gaitCode | $00
      IF gaitCode = 0 THEN
        gaitCode = $03
      ENDIF
    CASE 821 TO 999
      gaitCode = gaitCode | $02
  ENDSELECT
  GOTO Parse_GaitCode
ENDIF