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

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

'-----[ I/O Definitions ]-------------------------------------------------
PSC             PIN     15              ' PSC module
Baud            CON     1021            ' 2400 baud

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

'-----[ Walking Variables ]-----------------------------------------------
PSCPort         VAR     Byte            ' Servo addresses
ramp            VAR     Byte            ' Ramp used in SEROUT
temp            VAR     Word            ' Temp Var
position        VAR     Word

'-------------------------------------------------------------------------
' Test 3 servo joints that make up a leg
' Default
' PSCPort = 0 -> leg1 horizontal servo
' PSCPort = 1 -> leg1 lift servo
' PSCPort = 16 -> leg1 ankle
' position = 750 -> servo center
'
' EXAMPLE 1:
' To move the ankle of leg1 alter the position value below.
'...
' PSCPort = 16
' position = 500
'...
'
' Position values can range from 250 to 1250.  Read your PSC manual!
' Position values of 500 to 1000 should be enough to see leg movement.
'
' EXAMPLE 2:
' Send a 1.5ms pulse to leg6 joints. Read your PSC manual!
'
'...
'  PSCPort = 10
'  position = 750
'  GOSUB Write_Joint
'
'  PSCPort = 11
'  position = 750
'  GOSUB Write_Joint
'
'  PSCPort = 21
'  position = 750
'...
'-------------------------------------------------------------------------

Main:
  ramp = $A

  PSCPort = 0
  position = 750
  GOSUB Write_Joint

  PSCPort = 1
  position = 750
  GOSUB Write_Joint

  PSCPort = 16
  position = 750
  GOSUB Write_Joint
END

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