'-----[ Adjust_Bicept.bsp ] -------------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... Adjust_Bicept.bsp
'   Purpose.... Align upper pivot deck servos
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 6/1/2005
'   Updated....
'
' Hardware:
' (1) SG5/6-UT Robotic Arm
' (1) Parallax Basic Stamp P Module
' (1) Parallas BOE
' (1) Parallax Servo Controller (PSC)
'
'=========================================================================
' Getting Started
'=========================================================================
' PLEASE verify the PSC to joint connections below.  They might be
' different from your assembly guide instructions.
'
' Base...........PSC Channel 0
' Bicep..........PSC Channel 1
' Elbow..........PSC Channel 2
' Wrist..........PSC Channel 3
' WristRotate....PSC Channel 4 (SG6 Connection)
' Gripper........PSC Channel 5
' RightBicep.....PSC Channel 6
'
'=========================================================================
' Program Overview
'=========================================================================
' This program is used to programmatically adjust the two bicep
' servos in case they cannot be aligned physically. This step
' is crucial to ensure long servo life and low current consumption.
' If the servos are not aligned properly, they will run hot and
' use more current than what's required for normal operation.
'
'=========================================================================
' Aligning the bicep servos
'=========================================================================
' 1. Download this program "as is" to your basic stamp.  By default the
'    bicep servos are programmed to center.  Notice the debug screen
'    displays:
'
'    RightBicepOffset Constant Declaration:
'    --------------------------------------

'    RightBicepOffset   CON    0
'
' 2. Look for any differences between the biceps servos positions.  If
'    they align perfectly you're done.  If not continue to step 3
'    physical adjustments.
'
' 3. Physically adjust just one biceps to match the other biceps position
'    as much as possible.  Remove one bicep from the servo spindle,
'    rotate it, and secure it back onto the servo spindle.
'
' 4. Look for any differences between the biceps servos positions.  If
'    they align perfectly you're done.  If not continue to step 5
'    Programmatic adjustments.
'
' 5. Find this section of code below.
'    rightBicepPos = 750
'    Change the 750 value to something else, like 745, download this code
'    and look for any differences in the bicep servo positions.  Continue
'    to programmatically adjust the rightBicepPos variable until the
'    biceps are as close to parallel as possible.
'
' 6. Write down the constant declaration statement on the debug screen.
'    You will use this value in all STAMP code written for the arm.
'    The value could be a negative number.
'
'
' Notes:
' Servos can vary 15 to 20 steps from each other. The rightBicepPos
' could be +/-20 units from the other bicep servo. If your getting
' values above 20 you should go back to step 3 and physically
' adjust the bicep.
'
'-------------------------------------------------------------------------

#IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN
  Baud              CON     1021            ' BS2p 2400 baud
#ELSE
  Baud              CON     33164           ' BS2 2400 baud
#ENDIF

PSC                 CON     15              ' PSC Module
Bicep               CON     1               ' Left Bicep PSC channel
RightBicep          CON     6               ' Right Bicep PSC channel
ramp                CON     $F              ' Servo rotation speed

bicepPos            VAR     Word            ' Bicept Position
rightBicepPos       VAR     Word            ' Right Bicept Position
diff                VAR     Word            ' Difference in Position

'Record your RightBicepOffset constant declaration.
RightBicepOffset    CON     -10

bicepPos = 750

'===================================================
' Adjust the value below to align the servos bracket
rightBicepPos = 750
'===================================================

Write_PSC:
  SEROUT PSC,Baud,["!SC",Bicep, ramp,
         bicepPos.LOWBYTE, bicepPos.HIGHBYTE, CR]

  SEROUT PSC,Baud,["!SC",RightBicep, ramp,
         rightBicepPos.LOWBYTE, rightBicepPos.HIGHBYTE, CR]

 diff = rightBicepPos - bicepPos

 DEBUG "RightBicepOffset Constant Declaration:", CR,
       "--------------------------------------", CR,CR,
       "RightBicepOffset   CON   ", SDEC diff

END