CON
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000

  ' Set pins and Baud rate for XBee comms
  XB_Rx     = 0    ' XBee DOUT
  XB_Tx     = 1    ' XBee DIN
  XB_Baud   = 9600 ' XBee Baud Rate

  RCServo       = 7

  CR = 13

OBJ
   XB    : "XBee_Object_2"

Pub  Start | DataIn, tInc
  XB.start(XB_Rx, XB_Tx, 0, XB_Baud)   ' Initialize comms for XBee

  repeat
    DataIn := XB.Rx                    ' Accept incoming byte
    If DataIn == "!"                   ' If start delimiter
      DataIn := XB.RxDecTime(500)      ' Accept value for Frequency
      if DataIn <> -1                  ' If wasn't timeout, hand data to Servo
        Servo(DataIn)

Pub Servo(pulse) | tInc, tC, tPulse, t

ctra [30..26] := %00100
ctra [8..0] := RCServo

frqa := 1
dira[RCServo]~~

tInc := clkfreq/1_000_000
tC   := tInc * 20_000
tPulse := tInc * pulse
t    := cnt

  repeat 100
    phsa := -tPulse
    t += tC
    waitcnt(t)