' {$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)
'
'=========================================================================
'Getting Started
'=========================================================================
' Base code for the HexCrawler HDATS walking gaits
'
'
'
'
'---- [Programming Notes] ------------------------------------------------

'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15              ' PSC module
Baud            CON     1021            ' 2400 baud

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

' Adjustable horizontal servo speed
HRamp           CON     $A
' Adjustable Vertical ramp speed
VRamp           CON     $7
' Ankle speed as a function of horizontal speed
ARamp           CON     HRamp+$6

'-----[ Horizontal Table Constants ]--------------------------------------
'Columns           CON      3
HT_Rows         CON      3
HT_Row          CON      3

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

'-----[ Walking Variables ]-----------------------------------------------

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
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

'---- [EEPROM Data] ------------------------------------------------------

BoHT DATA  Word 670,  40,
           Word 770,  0
EoHT DATA  Word 870,  40

BoCHT DATA 2, Word 850,
           2, Word 750
EoCHT DATA 2, Word 650

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

Initialize:
  dirBit = 0                            ' 0 = Forward  1 = reverse
                                        ' 0 = Crab Right 1 = Left
  crabBit = 1                           ' 0 = Tripod  1 = Crab
  angleDirBit = 0                       ' Change horz leg positions
  liftBit = 0                           ' Do not change
  hOffset = 0                           ' 0 = horizontal center
  'gaitCode = $0F                        ' Gait Code

  IF crabBit = 0 THEN
    ptrEEPROM = BoHT                    ' set pointer
  ELSE
    ptrEEPROM = BoCHT
  ENDIF
  GOSUB Lift_Lower

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

  FOR servoAddr = 2 TO 10 STEP 4
    IF crabBit = 0 THEN
      GOSUB Tri_Positions
    ELSE
      GOSUB Crab_Positions
    ENDIF
  GOSUB Write_PSC
  NEXT

' 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

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:
  ramp = HRamp
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   eepromData(0), eepromData(1), CR]
  'ramp = ARamp
  SEROUT PSC,Baud,["!SC",((servoAddr/2)+16), ramp, temp.LOWBYTE,
                   temp.HIGHBYTE, CR]
  ramp = HRamp
RETURN

'-------------------------------------------------------------------------
Lift_Lower:
  PUT 2, Word ptrEEPROM
  ramp = VRamp
' Position EEPROM pointer
  IF dirBit = 0 THEN
    ptrEEPROM = BoLLT
    FOR servoAddr = 1 TO 9 STEP 4
      GOSUB Vertical_Servos
    NEXT
    PAUSE 200
    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
    PAUSE 200
    FOR servoAddr = 1 TO 9 STEP 4
      GOSUB Vertical_Servos
    NEXT
  ENDIF

  dirBit = dirBit + 1
  GET 2, Word ptrEEPROM
RETURN

Vertical_Servos:
  READ ptrEEPROM, temp.LOWBYTE, temp.HIGHBYTE
  SEROUT PSC,Baud,["!SC",servoAddr, ramp, temp.LOWBYTE, temp.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + VT_Row
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