' {$STAMP BS2}
' {$PBASIC 2.5}

'===================== Declarations ======================

counter VAR Byte
servo  VAR Byte
PinNumber2 VAR Byte
PinNumber  VAR   Byte
PinState   VAR   Byte
PinNumber1  VAR   Byte
PinState1   VAR   Byte

'======================== Main Routine ===================

Main:

DO

 GOSUB LED1
 GOSUB LED2
GOSUB Servo1         ' Go to Servo events

LOOP

'====================== Subroutines ======================

'======================== LED1 ===========================

LED1:
SERIN 16,16780,[WAIT(255),PinNumber,PinState]

' If PinState=0 Then go to GoLow
' otherwise go to GoHigh
BRANCH PinState,[GoLow,GoHigh]

' Set The pin low
GoLow:
LOW PinNumber
RETURN

' Set the pin high
GoHigh:
HIGH PinNumber

RETURN


'======================== LED2 ===========================

LED2:
SERIN 16,16780,[WAIT(255),PinNumber1,PinState1]

' If PinState=0 Then go to GoLow
' otherwise go to GoHigh
BRANCH PinState1,[GoLow1,GoHigh1]

' Set The pin low
GoLow1:
'LOW PinNumber1
RETURN


' Set the pin high
GoHigh1:
HIGH PinNumber1
RETURN

'======================== Servo1 ==========================

Servo1:
 FOR counter = 1 TO 200
 SERIN 16,16780,[WAIT(255),PinNumber2, servo]
 PULSOUT PinNumber2,(servo*15)
 NEXT
RETURN 