' {$STAMP BS2}
' {$PBASIC 2.5}

' -------------------------------------------------------------------
' I/O Definitions
' -------------------------------------------------------------------
Servo CON 12 ' servo pin
RX    CON 16 ' serial pin

' -------------------------------------------------------------------
' Constants
' -------------------------------------------------------------------
Bauda CON 16780
Baudb CON 16468
servo_min CON 550
servo_max CON 1000

' -------------------------------------------------------------------
' Variables
' -------------------------------------------------------------------
serialdata VAR Word
finalPos VAR Word
sPos VAR Word


' -------------------------------------------------------------------
' Program Code
' -------------------------------------------------------------------
Init:
finalPos = 750
sPos = finalPos

Main:
SERIN RX,Bauda,15,update, [WAIT("!V"), DEC serialdata]
finalPos =  ((((servo_max - servo_min)/100) * serialdata) + servo_min)
update:
IF finalPos < sPos THEN
  sPos = sPos - 1
ELSEIF finalPos > sPos THEN
  sPos = sPos + 1
ENDIF
PULSOUT Servo, (sPos) ' move the servo
GOTO Main