'---- [TuneQuadRodLegs.bs2 Version 1.0]------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... TuneQuadRodLegs.BS2
'   Purpose.... Adjust QuadRod Legs
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 06 November 2005
'   Updated.... 06 November 2005
'   Version.... 1.0
'
'======= Updates =======================================
' 1. Programmatic leg adjustment
'=======================================================
'
' Use the TuneQuad.BS2 program to programmatically tune the
' horizontal leg joints.  After physically tuning the
' QuadRod legs, you probably found it was impossible
' to physically position all of the legs equally.
'
' Therefore, CrustCrawler created a process to finely tune your
' QuadRod.  Why?  The QuadRod is a precision device.
' It requires that all legs move an equal distance
' as they arc through their range of horizontal motion.
' If one leg is off only a fraction the robot will favor
' one side over the other.
'
' For example, the robot might gradually turn to the left
' or the right while walking forward.
'
' Process:
' 1. Adjust the legs according to the QuadRod assembly guide.
' 2. Adjust the leg programmatically
'
' Find these commands in the code below
' Center1         CON     750     ' Leg1 horizontal servo
' Center2         CON     750     ' Leg2 horizontal servo
' Center3         CON     750     ' Leg3 horizontal servo
' Center4         CON     750     ' Leg4 horizontal servo
'
' In PSC terms servo center is 750 or a 1.5ms pulse.
' See your PSC documentation for a brief description of
' servo operation.
'
' Center1 and Center2 constants adjust the right horizontal
' servos (legs 1 and 2).
'
' Center3 and Center4 adjust the left horizontal
' servos (legs 3 and 4).
'
'      **Right Side**
'      \          /
'       \        /
' F  |--------------|
' R  |  1        2  |
' 0  |  3        4  |
' N  |--------------|
' T     /        \
'      /          \
'      **Left Side**
'
' Increasing the Center1 or Center2 constants move the right
' legs forward.  Decreasing the constants move the right
' legs toward the rear of the QuadRod.
' The opposite is true for the left horizontal servos.
'
' How to adjust the legs.
' Ideally the front legs will point about 10-15 degrees forward and
' the rear legs will pont about 10-15 degrees toward the rear of the ROD
'
' Programmatic Adjustments
' 1. Start with leg one.
' 2. Change the Center# constant value for that leg.
' 3. Save and download the program to the BS2.
' 4. Verify that the leg is now is positioned correctly
'    if not repeat the steps above.
' 5. Repeat the steps above for all legs that require adjustment.
'
'
'-----[ Example Horizontal Leg Constants ]----------------------------------------
' My center values. Yours will be different
' Right Side - Increase value to move leg forward
'Center1         CON     900  'Front
'Center2         CON     700  'Back
' Left Side
'Center3         CON     600  'Front
'Center4         CON     800  'Back
'
' After you center the legs programmatically, use these values in all
' QuadRod programs.
'
'-------------------------------------------------------------------------

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

'-----[ Walking Variables ]-----------------------------------------------
servoAddr       VAR     Byte            ' Servo addresses
ptrEEPROM       VAR     Word            ' Gait select
servoPosition   VAR     Word            ' Servo Position
ramp            VAR     Byte            ' Ramp used in SEROUT

'-----[ General Leg Constants ]-------------------------------------------
LiftRamp        CON     $1     ' Very fast ramp for lifting leg
Fast            CON     $F

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

'-----[ Vertical Leg Constants ]------------------------------------------
RaiseRight      CON     300
LowerRight      CON     1200
RaiseLeft       CON     1200
LowerLeft       CON     300

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

'Initialize EEPROM Pointer and servo address variables
ptrEEPROM = Adjust
READ ptrEEPROM, servoAddr

' Check for the end of EEPROM data ($FF).
' Check for odd or even servo address.
' Odd servo addresses are vertical servos.
' Even servo address are horizontal servos.
' Assign ramp value to vertical and horizontal servos.
' Call PSC_Write sub routine.
Walking_Engine:
  DO WHILE ServoAddr <> $FF       ' Do while not at the end of EEPROM data
    IF (ServoAddr // 2) = 1 THEN  ' Odd servo address = Vertical servo
      ramp = LiftRamp             ' Vertical servo ramp speed
    ELSE
      ramp = Fast                 ' Horizontal servo ramp speed
    ENDIF
    GOSUB PSC_Write               ' Write to PSC
  LOOP
END

'----- [Serial Out EEPROM values to PSC] ---------------------------------
' Read three EEPROM bytes into variables
' Write to PSC
' Update EEPROM pointer to next servo address
' Read next servo address into servoAddr
' Pause to allow servo to reach position
PSC_Write:
  READ ptrEEPROM, servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  SEROUT PSC,Baud,["!SC",ServoAddr, Ramp,
         servoPosition.LOWBYTE,
         servoPosition.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + 3
  READ ptrEEPROM,servoAddr
  PAUSE 300
RETURN