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

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

'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15              ' PSC module
#SELECT $STAMP
  #CASE BS2SX, BS2P
    Baud        CON     1021 + $8000
  #CASE BS2PX
    Baud        CON     1646 + $8000
  #CASE #ELSE
    Baud        CON     396 + $8000
#ENDSELECT

'-----[ Leg Constants ]------------------------------------------
Center          CON     750

'-----[ Walking Variables ]-----------------------------------------------
servoAddr       VAR     Byte            ' Servo addresses
ptrEEPROM       VAR     Word            ' Gait select
temp            VAR     Word            ' Temp Var
servoPosition   VAR     Byte(6)         ' Servo Position
ramp            VAR     Byte            ' Ramp used in SEROUT
position        VAR     Word

'-------------------------------------------------------------------------
ramp = $9

Joint_Test:
  servoAddr = 0
  position = 750
  GOSUB Write_Joint

  servoAddr = 1
  position = 750
  GOSUB Write_Joint

  servoAddr = 16
  position = 750
  GOSUB Write_Joint
  END


Write_Joint:
  SEROUT PSC,Baud,["!SC",ServoAddr,
         Ramp,position.LOWBYTE,
         position.HIGHBYTE, CR]
RETURN