'{$STAMP BS2}
'{$PBASIC 2.5}
'
'
' LED   gaitCode
' (0)   $00 - Home
' (1)   $01 - Spin Left
' (2)   $02 - Spin Right
'
' LED   gaitCode                    LED  gaitCode
' (3)   $10 - Forward Fast         (6)   $20 - Forward
' (4)   $11 - Fast Forward Left    (7)   $21 - Forward Left
' (5)   $12 - Fast Forward Right   (8)   $22 - Forward Right
'
' LED   gaitCode                   LED   gaitCode
' (9)   $30 - Backward             (C)   $40 - Fast Backward
' (A)   $31 - Backward Left        (d)   $41 - Fast Backward Left
' (b)   $32 - Backward Right       (E)   $42 - Fast Backward Right
'                                  (F)   Open
'  I/O Definitions
PSC             PIN     15
ModeBtn         PIN     8
StartBtn        PIN     12
segments        VAR     OUTL
Baud            CON     33164
' ADC0831 I/O
FrontIR         CON     9
DIO_10          CON     10
CLK             CON     11

' Transform Function Constants
Slope           CON     3200
y_Intercept     CON     3
k               CON     4

' Button Variables
btns            VAR     Nib
btn1            VAR     btns.BIT0
btn2            VAR     btns.BIT1
idx             VAR     Nib
btnIndx         VAR     Byte

' Button states
Pressed         CON     1
NotPressed      CON     0

' Walking Variables
counter         VAR     Nib
servoAddr       VAR     Byte
gaitCode        VAR     Byte
tempCode        VAR     Byte
ramp            VAR     Byte
rightRamp       VAR     Byte
leftRamp        VAR     Byte
distTemp        VAR     Byte
distance        VAR     Byte
ptrEEPROM       VAR     Word
servoPosition   VAR     Word

' Adjustable Ramp Values
'Off  fast to slow
' 0    $1  -- $3F(63)
LiftRamp        CON     $1
VeryFast        CON     $8
Fast            CON     $A

' Horizontal Leg Constants
'Servo center
Center1         CON     760
Center2         CON     815
Center3         CON     755
Center4         CON     770

' Stride Calculations
Stride          CON     150
delay           CON     Stride/5

Leg1Center      CON     Center1
Leg1Forward     CON     Center1-Stride
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
RaiseRightF     CON     725
LowerRightF     CON     1220
RaiseRightR     CON     1150
LowerRightR     CON     700
RaiseLeftF      CON     430
LowerLeftF      CON     1000
RaiseLeftR      CON     460
LowerLeftR      CON     1030

' 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 RaiseRightF,         $07,Word RaiseLeftR,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $01,Word LowerRightF,         $07,Word LowerLeftR,
             $03,Word RaiseRightR,         $05,Word RaiseLeftF,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $03,Word LowerRightR,         $05,Word LowerLeftF,
             $FF ' end of forward

' Walk Backward
Back    DATA $01,Word RaiseRightF,         $07,Word RaiseLeftR,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $01,Word LowerRightF,         $07,Word LowerLeftR,
             $03,Word RaiseRightR,         $05,Word RaiseLeftF,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $03,Word LowerRightR,         $05,Word LowerLeftF,
             $FF 'end of back

' Left Turn EEPROM values
LTurn   DATA $01,Word RaiseRightF,         $07,Word RaiseLeftR,
             $00,Word Leg1Forward+Stride, $06,Word Leg4Back+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $01,Word LowerRightF,         $07,Word LowerLeftR,
             $03,Word RaiseRightR,         $05,Word RaiseLeftF,
             $00,Word Leg1Center+Stride,  $06,Word Leg4Center+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $03,Word LowerRightR,         $05,Word LowerLeftF,
             $FF ' end of left turn

' Right turn EEPROM Values
RTurn   DATA $01,Word RaiseRightF,         $07,Word RaiseLeftR,
             $00,Word Leg1Forward,        $02,Word Leg2Center-Stride,
             $04,Word Leg3Center-Stride,  $06,Word Leg4Back,
             $01,Word LowerRightF,         $07,Word LowerLeftR,
             $03,Word RaiseRightR,         $05,Word RaiseLeftF,
             $00,Word Leg1Forward,        $02,Word Leg2Back-Stride,
             $04,Word Leg3Forward-Stride, $06,Word Leg4Back,
             $03,Word LowerRightR,         $05,Word LowerLeftF,
             $FF  ' end of right turn

'                Up                Center           Down
Adjust  DATA $01,Word RaiseRightF, $00,Word Center1, $01,Word LowerRightF
        DATA $03,Word RaiseRightR, $02,Word Center2, $03,Word LowerRightR
        DATA $05,Word RaiseLeftF,  $04,Word Center3, $05,Word LowerLeftF
        DATA $07,Word RaiseLeftR,  $06,Word Center4, $07,Word LowerLeftR,
        $FF ' end of adjust legs
' End EEPROM Data

' Initialize
DIRL = %01111111
gaitCode = $10
btnIndx = $03
segments = $3D
HIGH FrontIR
GOTO Select_GaitCode

' Gait Selection
Parse_GaitCode:
  SELECT gaitCode
    CASE $00
      ptrEEPROM = Adjust
      rightRamp = Fast
      leftRamp = Fast
      GOTO Walking_Engine
    CASE $01
      ptrEEPROM = LTurn
      rightRamp = VeryFast
      leftRamp = VeryFast
      GOTO Walking_Engine
    CASE $02
      ptrEEPROM = RTurn
      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

' LowNib
' left right
' 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
    END
  ENDIF
'Check for a button press
  GOSUB Read_Buttons
  IF (btns.BIT0 = Pressed) OR (btns.BIT1 = Pressed) THEN
    GOTO Select_GaitCode
  ELSE
    GOTO  Get_IR
  ENDIF

' Sub routines

' 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
Select_GaitCode:
  DO WHILE (btns.BIT0 = NotPressed) OR (btns.BIT1 = NotPressed)
    IF (btns > %00) AND (btns < %11) THEN
      btnIndx = btnIndx + btn1 // 15
      btnIndx = btnIndx + (14 * btn2) // 15
      READ btnIndx, segments
      READ gait + btnIndx, gaitCode
      PAUSE 250
    ENDIF
    GOSUB Read_Buttons
  LOOP
  GOTO Parse_GaitCode

' Read Buttons
Read_Buttons:
  btns = %0011
  FOR idx = 1 TO 5
    btns.BIT0 = btns.BIT0 & ModeBtn
    btns.BIT1 = btns.BIT1 & StartBtn
    PAUSE 5
  NEXT
RETURN

' IR Routines
  Get_IR:
    distTemp = 0
    distance = 0
    FOR counter = 1 TO 3
      GOSUB ADC0831_Reading
      distance = distance + (distTemp/3)
      PAUSE 50
    NEXT
    GOSUB ADC0831_Distance_Calc
     SELECT distance
     CASE < 15
      IF gaitCode = $10 THEN
      tempCode = $40
      ELSEIF gaitcode = $20 THEN
      tempCode = $40
      ELSEIF gaitcode <> $10 AND gaitCode <> $20 THEN
      tempCode = $42
      ENDIF

      CASE < 30
        IF gaitCode = $10 THEN
      tempCode = $40
      ELSEIF gaitcode = $20 THEN
      tempCode = $42
      ELSEIF gaitcode <> $10 AND gaitCode <> $20 THEN
      tempCode = $32
      ENDIF

      CASE < 45
        IF gaitCode = $10 THEN
      tempCode = $42
      ELSEIF gaitcode = $20 THEN
      tempCode = $32
      ELSEIF gaitcode <> $10 AND gaitCode <> $20 THEN
      tempCode = $01
      ENDIF

      CASE < 60
        IF gaitCode = $10 THEN
      tempCode = $11
      ELSEIF gaitcode = $20 THEN
      tempCode = $21
      ELSEIF gaitcode <> $10 AND gaitCode <> $20 THEN
      tempCode = $01
      ENDIF

      CASE < 70
        IF gaitCode = $10 THEN
      tempCode = $21
      ELSEIF gaitcode = $20 THEN
      tempCode = $21
      ELSEIF gaitcode <> $10 AND gaitCode <> $20 THEN
      tempCode = $20
      ENDIF

      CASE ELSE
        tempCode = $10
    ENDSELECT

    IF tempCode = gaitCode THEN
      ptrEEPROM = ptrEEPROM - 48
      GOTO Walking_Engine
    ELSE
      gaitCode = tempCode
      GOSUB Get_Segments
      GOTO Parse_GaitCode

    ENDIF

'Get ADC0831 8-bit data
  ADC0831_Reading:
    LOW FrontIR
    SHIFTIN DIO_10,CLK,
            MSBPOST,[distTemp\9]
    HIGH FrontIR
  RETURN

'Linearizing function to calculate
'GP2D12 distance reading
  ADC0831_Distance_Calc:
    distance = (Slope /(distance + y_Intercept)) - k
  RETURN

'Update LED display
  Get_Segments:
    btnIndx = Gait
    READ btnIndx, tempCode
    DO WHILE tempCode <> gaitCode
      btnIndx = btnIndx + 1
      READ btnIndx, tempCode
    LOOP
    READ btnIndx-Gait, segments
  RETURN