'-----[ Tune HexCrawler HDATS ]-------------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... Tune_Hex_HDATS.bsp
'   Purpose.... Adjust the HEXCRawler HDATS leg joints.
'   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. Power the BOE/BPSp and PSC
' 2. Run this program
'
'=========================================================================
' First Time Use
'=========================================================================
' On power up the PSC will center all servos.  If you ran the
' Tune_Hex_HDATS.bsp code and the legs are not centered then press
' the BOE reset button.
'
' It is recommended that the robot is off the ground on the initial
' start up or until the leg have reached their tuning positions.  Not
' doing so will cause unnecessary stress on the servo joints.
'
' Adjust the legs according to the assembly guide.
'
'      **Right Side**
'    0,1,15
' F     |     |     |
' O    _|_   _|_   _|_
' R   / 1 \_/ 2 \_/ 3 |
' W  |                |
' A  |     _     _    |
' R   \_4_/ \_5_/ \_6_|
' D     |     |     |
'       |     |     |
'
'=========================================================================
' Troubleshooting
'=========================================================================
' 1. Nothing happens after running the program.
'
' Check all connections and power.  Verify that the PSC and BOE/BS2p
' have power. Make sure that the servos are connected correctly to the
' PSC.  Ground on the PSC is always facing toward the outside of the PSC.
'
' 2. One or more joints are not close to centered or lowered.  Or
'    the robot does not walk well after running the HexHDATS_PLCD.bsp
'
' Check the servo connections.  You might have a horizontal leg joint
' swapped with a vertical leg joint.
'
'-------------------------------------------------------------------------
PSC             PIN   15              ' PSC module
Baud            CON   1021+$8000      ' 2400 baud

HServos         CON   750             ' Horizontal position
LVservo         CON   1200            ' Left lower position
RVservo         CON   300             ' Right lower position
Ankle           CON   750             ' Ankle center
ramp            CON   $A              ' Servo speed

servoAddr       VAR   Byte            ' Servo address
temp            VAR   Word            ' Temp variable

' Adjust horizontal servos
temp = HServos
FOR servoAddr = 0 TO 10 STEP 2
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   temp.LOWBYTE, temp.HIGHBYTE, CR]
NEXT

' Adjust left vertical servos
temp = LVservo
FOR servoAddr = 1 TO 5 STEP 2
    SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   temp.LOWBYTE, temp.HIGHBYTE, CR]
NEXT

' Adjust right vertical servos
temp = RVservo
FOR servoAddr = 7 TO 11 STEP 2
    SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   temp.LOWBYTE, temp.HIGHBYTE, CR]
NEXT

' Adjust ankle servos
temp = Ankle
FOR servoAddr = 16 TO 21 STEP 1
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   temp.LOWBYTE, temp.HIGHBYTE, CR]
NEXT

END