'---- [TuneQuad.bs2 Version 1.1]------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
' Ver 1.1
'
'   File....... TuneQuad.BS2
'   Purpose.... Adjust QuadCrawler Legs
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 16 April 2004
'   Updated.... 19 November 2004
'   Version.... 1.2
'
'======= Updates =======================================
' 1. Programmatic leg adjustment
'=======================================================
'
' Use the TuneQuad.BS2 program to programmatically tune the
' horizontal leg joints.  After physically tuning the
' QuadCrawler legs, you probably found it was impossible
' to position all of the legs perpendicular to the body.
' This is because NOT all servos are created equal.
'
' Therefore, CrustCrawler created a process to finely tune your
' QuadCrawler.  Why?  The QuadCrawler 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 QuadCrawler 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**
'     +750-      +750-
' F     |        |
' O    _|_      _|_
' R   / 1 \____/ 2 |  R
' W  |             |  E
' A  |     ____    |  A
' R   \_3_/    \_4_|  R
' D     |        |
'       |        |
'     -750+      -750+
'     **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 QuadCrawler.
' The opposite is true for the left horizontal servos.
'
' Programmatic Adjustments
' 1. Pick a leg that is not perpendicular to the robot's body.
' 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 perpendicular the body
'    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
'Center1         CON     750 -> No change
'Center2         CON     800 -> Leg 2 forward 50 units
'Center3         CON     785 -> Leg 3 back 15 units
'Center4         CON     750 -> No change
'
' After you center the legs programmatically, use these values in all
' QuadCrawler 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