' {$STAMP BS2p}
' {$PBASIC 2.5}

'---- [Programming Notes] ------------------------------------------------
'

'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15              ' PSC module

#SELECT $STAMP
  #CASE BS2SX, BS2P
    Baud        CON     1021 + $8000
  #CASE BS2PX
    Baud        CON     1646 + $8000
  #CASE #ELSE
    Baud        CON     396 + $8000
#ENDSELECT

EEPROM_Block    CON     108

'-----[ Walking Variables ]-----------------------------------------------
ptrEEPROM       VAR     Word            ' Gait select
servoPosition   VAR     Byte(6)         ' Servo Position
servoAddr       VAR     Byte            ' Servo addresses
ramp            VAR     Byte            ' Ramp used in SEROUT
pointer         VAR     Byte            ' pointer to scatchpad
gaitCode        VAR     Byte
direction       VAR     gaitCode.LOWNIB
speed           VAR     gaitCode.HIGHNIB
counter         VAR     Nib             ' Count 0 to 5

'-----[ Leg Constants ]---------------------------------------------------
Raise           CON     600
Lower           CON     300
Center          CON     750

'-----[ Speed settings ] -------------------------------------------------
RampOffset      CON     $B
IncrSpeed       CON     RampOffset-$2

'---- [EEPROM Data] ------------------------------------------------------
D1 DATA       Word Center,    Word Lower, Word Center,    ' Start Stroke A
              Word Center+50, Word Lower, Word Center-50,
              Word Center-70, Word Lower, Word Center-50,
              Word Center,    Word Raise, Word Center-80, ' Mid Reset B
              Word Center-70, Word Raise, Word Center+10,
              Word Center+50, Word Raise, Word Center+10,

              Word Center,    Word Lower, Word Center-50, ' End Stroke A
              Word Center-70, Word Lower, Word Center+10,
              Word Center+50, Word Lower, Word Center+10,
              Word Center,    Word Lower, Word Center-50, ' Start Stroke B
              Word Center-70, Word Lower, Word Center+10,
              Word Center+50, Word Lower, Word Center+10,

              Word Center,    Word Raise, Word Center,    ' Mid Reset A
              Word Center+50, Word Raise, Word Center+50,
              Word Center-70, Word Raise, Word Center-50,
              Word Center,    Word Lower, Word Center,    ' End Stroke B
              Word Center,    Word Lower, Word Center-20,
              Word Center,    Word Lower, Word Center-20,
              $FF

D2 DATA       Word Center,    Word Lower, Word Center,    ' Start Stroke A
              Word Center+50, Word Lower, Word Center-10,
              Word Center,    Word Lower, Word Center-10,
              Word Center-50, Word Raise, Word Center,    ' Mid Reset B
              Word Center-50, Word Raise, Word Center,
              Word Center,    Word Raise, Word Center-10,

              Word Center-50, Word Lower, Word Center-50, ' End Stroke A
              Word Center+25, Word Lower, Word Center+40,
              Word Center+50, Word Lower, Word Center+40,
              Word Center-50, Word Lower, Word Center,    ' Start Stroke B
              Word Center-50, Word Lower, Word Center+50,
              Word Center,    Word Lower, Word Center-10,

              Word Center,    Word Raise, Word Center,    ' Mid Reset A
              Word Center+50, Word Raise, Word Center-10,
              Word Center,    Word Raise, Word Center-10,
              Word Center,    Word Lower, Word Center+50, ' End Stroke B
              Word Center,    Word Lower, Word Center,
              Word Center+50, Word Lower, Word Center+40,
              $FF

  PUT $0, $0,$4,$8,$6,$A,$2
  PUT $6, $2,$6,$A,$8,$0,$4
  PUT $C, $4,$8,$0,$A,$2,$6
  PUT $12,$6,$A,$2,$0,$4,$8
  PUT $18,$8,$0,$4,$2,$6,$A
  PUT $1E,$A,$2,$6,$4,$8,$0

gaitCode = $00
counter = 0

Parse_GaitCode:
  IF (direction//2) = 0 THEN
    ptrEEPROM = D1
  ELSE
    ptrEEPROM = D2
  ENDIF
  pointer = (direction/2)*6
  ramp = speed + RampOffset

  'DEBUG HEX ?gaitCode, CR, HEX ?pointer, CR, HEX ?ramp

Walking_Engine:
  READ ptrEEPROM, servoPosition(0), servoPosition(1),
                  servoPosition(2), servoPosition(3),
                  servoPosition(4), servoPosition(5)
  DO WHILE (servoPosition(0) <> $FF)
      GET counter+pointer, servoAddr
      GOSUB Write_PSC
      counter = (counter+1)//6
  LOOP
  ptrEEPROM = ptrEEPROM - EEPROM_Block
GOTO Walking_Engine

'-------------------------------------------------------------------------
Write_PSC:
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   servoPosition(0), servoPosition(1), CR]
  ramp = ramp - IncrSpeed
  servoAddr = (servoAddr+1)
  PAUSE 2
  SEROUT PSC,Baud,["!SC",servoAddr, ramp, servoPosition(2),
                   servoPosition(3), CR]
  ramp = ramp + IncrSpeed
  servoAddr = ((servoAddr-1)/2)+16
  PAUSE 2
  SEROUT PSC,Baud,["!SC",servoAddr, ramp, servoPosition(4),
                   servoPosition(5), CR]
  ptrEEPROM = ptrEEPROM + 6
  READ ptrEEPROM, servoPosition(0), servoPosition(1),
                  servoPosition(2), servoPosition(3),
                  servoPosition(4), servoPosition(5)
  PAUSE 2
RETURN