' {$STAMP BS2p}
' {$PBASIC 2.5}

'---- [Programming Notes] ------------------------------------------------

'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15              ' PSC module

#SELECT $STAMP
  #CASE BS2SX, BS2P
    N2400        CON     1021+$8000
  #CASE BS2PX
    N2400        CON     1646+$8000
  #CASE #ELSE
    N2400        CON     396+$8000
#ENDSELECT


'-------------------------------------------------------------------------

servoAddr       VAR     Byte            ' Servo addresses
ramp            VAR     Byte            ' Ramp used in SEROUT
position        VAR     Word

'-------------------------------------------------------------------------


ramp = $F             ' Ramp (speed)
servoAddr = 0         ' Channel 0

Main:
  position = 500      ' Position
  GOSUB Write_Joint   ' Send position command
  PAUSE 3000          ' Pause 3 seconds
  position = 1000     ' Position
  GOSUB Write_Joint   ' Send position command
  PAUSE 3000          ' Pause 3 seconds
GOTO Main             ' Loop

' Write to the PSC
Write_Joint:
  SEROUT PSC,N2400,["!SC",ServoAddr, Ramp,position.LOWBYTE, position.HIGHBYTE, CR]
RETURN