'-------- [Sg5 Arm Test] ------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File........ SG5 Arm Test
'   Purpose..... To test the arm after construction for basic
'                movement of all joints TO ensure
'                proper connections of electronics.
'   Author...... CrustCrawler Inc. (Alex Dirks)
'   E-mail...... support@crustcrawler.com
'   Web Site.... www.crustcrawler.com
'   Started .... July 10 2004
'   Updated..... July 12 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 ] -------------------------
Sdat     CON   4       ' 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 Ramp Values] ---------------------
' Sets the ramp speed of the PSC controller (0 - 63).
' The higher the number the slower the movement
ramp     CON  11

'---- [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)


'---- [Servo Position Variables] ------------------
position0   VAR     Word
position1   VAR     Word
position2   VAR     Word
position3   VAR     Word
position4   VAR     Word
position5   VAR     Word

' The basic range for the position
' variables are 400 - 1150 (+ Or -).

' Note: the parallal left and right bicept servos
' have a different value based On the values
' used to align the bicepts during construction.
' Subsitute the values that you used during
' the construction process.

'---- [Home position] ----
' Arm is in the upright position - gripper open
position0 = 780         ' Base servo
position1 = 750         ' Left bicept servo
position2 = 735         ' Right bicept servo
position3 = 620         ' elbow servo
position4 = 750         ' wrist servo
position5 = 1150        ' gripper servo

'---- [Arm Adjustable Movements] ----
' Un-comment each section below one at a time to observe
' different arm movements.
' You can change the servo position variables
' (400 to 1150) to obtain differnet arm positions.
'
'---- [Arm goes down towards object] ---------
'position1 = 500  'Left bicept servo
'position2 = 485  'Right bicept servo
'position4 = 780  'wrist servo

'---- [The gripper closes] ----
'position5 = 810 'gripper servo

'---- [The arm lifts back up with the object] ----
'position1 = 750  'Left bicept servo
'position2 = 735  'Right bicept servo

'---- [Rotates the arms base] ----
'position0 = 980  'Base servo

'---- [Puts the arm in the down position] ----
'position1 = 500  'Left bicept servo
'position2 = 485  'Right bicept servo

'---- [Opens the gripper] ----
'position5 = 1150 'gripper servo

'---- [Main Program] -------------------------------

SEROUT Sdat,baud,["!SC", Base, ramp, position0.LOWBYTE, position0.HIGHBYTE, CR]
PAUSE 500 ' Give servo time to reach position

SEROUT Sdat,baud,["!SC", LBicept, ramp, position1.LOWBYTE, position1.HIGHBYTE, CR]
SEROUT Sdat,baud,["!SC", RBicept, ramp, position2.LOWBYTE, position2.HIGHBYTE, CR]
PAUSE 500

SEROUT Sdat,baud,["!SC", Elbow, ramp, position3.LOWBYTE, position3.HIGHBYTE, CR]
PAUSE 500

SEROUT Sdat,baud,["!SC", Wrist, ramp, position4.LOWBYTE, position4.HIGHBYTE, CR]
PAUSE 500

SEROUT Sdat,baud,["!SC", Gripper, ramp, position5.LOWBYTE, position5.HIGHBYTE, CR]
PAUSE 500

END