'---- [S3_Demo.BS2] -------------------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... S3_Demo.BS2
'   Purpose.... Center the S3 X and Y servos.  Execute a
'               simple scan routine.
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 5 January 2005
'   Updated.... 5 January 2005
'
'   Hardware
'   (1) S3 Tilt Pan System
'   (1) Parallax Basic Stamp 2
'   (1) ParalaxBoard of Education (BOE)
'   (1) Parallax Servo Controller (PSC)
'
' Run the S3_Demo.bs2 programs to
' 1. Physically center the X and Y servos
' 2. Programmatically center the X and Y servos
' 3. Execute a simply scanning routine.
'
'=========================================================================
' Getting Started
'=========================================================================
' 1. SERVO CONNECTIONS
' This program assumes that the s3's X (horizontal) servo is connected
' to PSC channel 12 and the Y (vertical) servo is connected to
' channel 13.  To change the default servo connections, find
' the X_Servo and Y_Servo constant declarations in the code below
' under the [I/O Definition] section. Change the constants to
' match your S3 servos to PSC channel connections.
'
' 2. X AND Y SERVO CENTER POSITIONS (DEFAULT)
' The default setting for servo center in 750 or  1.5ms pulses
' from the PSC.  Use this center to physically adjust your S3
' according to the S3 assembly guide.
'
' 3. PROGRAMMATIC X AND Y SERVO CENTER
' After physically adjusting the S3 servos, you might have found
' that the S3 is not exactly centered.  Adjust the X_ServoCenter
' and Y_ServoCenter constants in the [S3 Constants] section to
' programmactically center the S3.
'
' 4. RUNNING THE SIMPLE SCAN
' Find the [Main Routine] section below.  Remove the comment from
' 'ptrEEPROM = Scan and run the program. This will cause the S3 to
' scan left, right, up, and down.
'
'-------------------------------------------------------------------------

' -----[ I/O Definitions ]------------------------------------------------
#IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN
  Baud  CON     1021                    ' 2400 baud
#ELSE
  baud  CON     33164                   ' 2400 baud
#ENDIF
PSC             PIN     15              ' PSC module
X_Servo         CON     12              ' Horizontal Servo Address
Y_Servo         CON     13              ' Vertical Servo Address

'---- [S3 Constants] -----------------------------------------------------
'ServoCenter     CON     750             ' Servo Center
X_ServoCenter   CON     750             ' Horizontal servo center
Y_ServoCenter   CON     750             ' Vertical servo center
verticalIncr    CON     200             ' Vertical Increment
horizontalIncr  CON     400             ' Horizontal Increment

' Adjustable Values
lookLeft        CON     X_ServoCenter - horizontalIncr
lookRight       CON     X_ServoCenter + horizontalIncr
EOD             CON     $FF             ' End of Data
Ramp            CON     $A              ' Servo Speed

'---- [Servo Position Variables] -----------------------------------------
servoPosition   VAR     Word            ' Servo Position
ptrEEPROM       VAR     Word            ' EEPROM Pointer
servoAddr       VAR     Byte            ' Servo Addresses
S3Counter       VAR     Nib             ' Loop Counter

'---- [EEPROM data] ------------------------------------------------------
' EEPROM is read in 3 byte sections
' servo address | LOWBYTE servo position | HIGHBYTE servo position
Scan   DATA X_Servo, Word lookLeft,
            X_Servo, Word X_ServoCenter,
            X_Servo, Word lookRight,
            X_Servo, Word X_ServoCenter,
            EOD

Center DATA X_Servo, Word X_ServoCenter,
            Y_Servo, Word Y_ServoCenter,
            EOD

' Initialize counter
S3Counter = 0

'--- [ Main Routine ] ----------------------------------------------------
Main:
  ptrEEPROM = Center                   ' Center Servos
  'ptrEEPROM = Scan                     ' Simple Scan

  READ ptrEEPROM,servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  DO WHILE servoAddr <> EOD             ' Loop while not end of EEPROM
    GOSUB PSC_Write                     ' Write to the PSC
  LOOP
  IF ptrEEPROM = Center + 6 THEN        ' End if center is selected
    END
  ELSE
    IF S3Counter < 4 THEN               ' Simple scan
      LOOKUP S3Counter,[Y_ServoCenter + verticalIncr,
             Y_ServoCenter, Y_ServoCenter - verticalIncr,
             Y_ServoCenter], servoPosition
      SEROUT PSC,Baud,["!SC",Y_Servo, Ramp,
             servoPosition.LOWBYTE,
             servoPosition.HIGHBYTE, CR]
      S3Counter = S3Counter+1
    ELSE
      S3Counter = 0
    ENDIF
  ENDIF
GOTO Main


'----- [ Sub Routines ] --------------------------------------------------
PSC_Write:
  SEROUT PSC,Baud,["!SC",ServoAddr, Ramp,
         servoPosition.LOWBYTE,
         servoPosition.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + 3
  READ ptrEEPROM,servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  PAUSE 1500
RETURN