'------- [SG5 Arm Pickup and Place Routine] --------------------------
'{$STAMP BS2p}
'{$PBASIC 2.5}

 '--------[Sg5 Arm Pickup and Place Routine] ------------------------

'File........ SG5 Arm Pickup and Place Routine
'Purpose..... This completely adjustable program allows the SG5 arm perform
'a simple "pickup" routine. The arm will reach out and down to an object (I used an
'empty soda can), pick it up, rotate to another position and place the can gently
'down. There is no "end" statement for this program, so the arm will repeat the
'operation.
'Author...... CrustCrawler Inc. (Alex Dirks)
'E-mail...... support@crustcrawler.com
'Web Site.... www.crustcrawler.com
'Started .... July 31 2004
'Updated..... July 31 2004
'--------------------------------------------------------------------------------
'When you connect your PSC controller to the arm servos, connect them in the
'following order:

'ch0 is the rotating base servo
'ch1 is the Left bicept servo
'ch2 is the Right bicept servo
'ch3 is the elbow servo
'ch4 is the wrist servo
'ch5 is the gripper servo

'
'--------------------------------------------------------------------------------------
' ----- [ I/O Definitions ] -------------------------
PSC      CON   15     ' PSC Module
' PSC servo channels
Base     CON   0      ' Rotating base
LBicept  CON   1      ' Left bicept
RBicept  CON   2      ' Right bicept
Elbow    CON   3      ' Elbow
Wrist    CON   4      ' Wrist
Gripper  CON   5      ' Gripper

'---- [Adjustable Baud Mode] ------------------------
Baud     CON  1021    ' BS2P24 baudmode (2400 baud, 8-bit, no parity)
'Baud     CON  396     ' BS2 baudmode (2400 baud, 8-bit, no parity)

'---- [Walking Variables] ------------------------------------------------
servoAddr       VAR     Byte            ' Servo addresses
Index           VAR     Word            ' pointer
servoPosition   VAR     Word            ' Servo Position
ramp            VAR     Byte            ' Ramp used in SEROUT
biceptLoop      VAR     Bit             ' Bicept control loop
delay           VAR     Word            ' Pause

'---- [Servo Address and Position EEPROM data] -----------------

'The data table below represents different servo addresses and actual positions
'of the servos in the BS2P24's EEPROM. In line one for example, $00 is the rotating
'base servo and "Word 800" represents the servo position which can vary from
'approximately 400 - 1150. The data in the table below is executed in
'sequencial order (one complete row before going to the next row.) The beauty of
'having your servo and position data in a table is that it allows for easy servo
'selection and fully adjustable movement. Additional routines can be added
'to the table to perform an unlimited amount customized arm movement routines.

Test DATA $00,Word 800, $01,Word 650, $02,Word 635, $03,Word 760,
          $04,Word 850, $05,Word 1150,
          $01,Word 450, $02,Word 435, $05,Word 980,
          $01,Word 650, $02,Word 635,$00, Word 1150,
          $01,Word 450, $02,Word 435, $05,Word 1150,
          $01,Word 650, $02,Word 635, $00, Word 800,

          $FF

'---- [End EEPROM Data] ----------------------------------------
' LBicept and RBicept servos make up the arm's
' bicept. Both servos must move at the same time
' or one of the servos will add stress to the other.
' This bit variable causes the
' Send_EEPROM_Info_to_PSC sub to grab
' 6 bytes from EEPROM instead of the normal
' 3 bytes.  This is accomplished with a
' Do... Loop While biceptLoop < 1
biceptLoop = 0
delay = 1500
'-----[ Main Walking Routine ]------------------------------------------
EEPROM_Reader:
' Assign EEPROM starting address
Index = Test
' Read PSC servo address at Index and assign
' ramp to each PSC servo position. Each servo's ramp
' can be adjusted to any speed. Pause statements are used to delay the
' arm movement while the gripper is either closing or opening.

  READ Index,ServoAddr
  DO WHILE servoAddr <> $FF
    SELECT servoAddr
      CASE Base
        ramp = $A
        delay = 1500
      CASE LBicept
        ramp = $B
        delay = 1500
        biceptLoop = 1
      CASE Elbow
        ramp = $A
        'delay = 1500
      CASE Wrist
        ramp = $B
        'delay = 1500
      CASE Gripper
        ramp = $B
        'delay = 1500
      ENDSELECT
    GOSUB Send_EEPROM_Info_to_PSC
  LOOP
  'end
  GOTO EEPROM_Reader

'----- [Serial Out EEPROM values to PSC] -------------------------------

'This is the main "engine" of the program. The data from the table and the
'pause and ramp routines are used in the SEROUT code below.

Send_EEPROM_Info_to_PSC:
  Move_Bicept:
  READ Index, servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  SEROUT PSC,Baud,["!SC",ServoAddr,ramp,servoPosition.LOWBYTE,servoPosition.HIGHBYTE,CR]
'DEBUG HEX ?servoAddr, HEX ?servoPosition.LOWBYTE, HEX ?servoPosition.HIGHBYTE
  Index = Index + 3
  IF biceptLoop = 1 THEN
    biceptLoop = 0
    'DEBUG "Bicept found",CR
    GOTO Move_Bicept
  ENDIF
  READ Index,servoAddr
  PAUSE delay
RETURN