CON
   
  _clkmode = xtal1 + pll16x                  
  _xinfreq = 5_000_000
   outPin =21
VAR 
  long ServoPos
  long stack [15]
  long stack2[50]
  long pcount
OBJ
  UTIL: "Utilities"
PUB Go 
  dira[outPin]~~
  cognew(MoveMotor(outPin),@Stack)
  
  repeat
     pcount:=UTIL.Read3202_0
     ServoPos:=7200+pcount*4
 
PUB MoveMotor(Pin)|LowTime,period
  dira[Pin]~~
  ctra[30..26]:=%00100                     
  ctra[5..0]:=Pin
  frqa:=1
  ServoPos:=0                           
  LowTime:=clkfreq/60 
  period:=cnt                                   

  repeat
    phsa:=-ServoPos
    period:=period+LowTime
    waitcnt(period)