CON

  _clkmode      = xtal1 + pll16x
  _xinfreq      = 5_000_000

  CW            = 1
  CCW           = -1
  
  POSD          = 0  'Index of positive direction constant.
  STAT          = 1  'Index of state variable.
  CURR          = 2  'Index of current angle variable.
  TARG          = 3  'Index of target angle variable.
  MAXSP         = 4  'Index of maximum speed variable.
  ERR           = 5  'Index of error variable (for debugging: doesn't need to be in hub).

  IDLE          = 0  'State values.
  NEWTARG       = 1
  MOVING        = 2
  HOLDING       = 3

VAR

  long PosDir, State, CurrentAngle, TargetAngle, MaxSpeed, Error  'Hub array
  long Stack[40], _1us, _5ms

PUB start | target, cogno, perror

  _1us := clkfreq / 1_000_000  'Compute clocks in 1 usec.
  _5ms := clkfreq / 200        'Compute clocks in 5 ms.

  PosDir := CCW
  State := IDLE
  cogno := cognew(do_servo(12, 14, @PosDir), @Stack)
  repeat target from 0 to 500 step 50
    go_to(target, 20)
    waitcnt(cnt + clkfreq)
  repeat target from 950 to 50 step 100
    go_to(target, 200)
    waitcnt(cnt + clkfreq)

PUB go_to(target, speed)

  'Go to target angle (0 - 999) by the shortest route at the selected speed (1 - 200)

  repeat while (State == NEWTARG or State == MOVING)
  TargetAngle := target
  MaxSpeed := speed
  State := NEWTARG

PUB do_servo (servo_pin, encoder_pin, vars) | t0, width, delay, t1, t2, t3, speed, perror, pangle, splimit, encmask, cerror, target, maxspd

  'Cog that keeps position and servo speed updated.

  dira[servo_pin]~~               'Set servo pin to be an output.
  encmask := 1 << encoder_pin     'Compute encoder mask from pin no.
  t0 := cnt                       'Get start time.
  repeat
    pangle := long[vars][CURR]    'Set previous angle.
    waitpne(encmask, encmask, 0)  'Wait encoder low.
    waitpeq(encmask, encmask, 0)  't1 := leading edge time.
    t1 := cnt
    waitpne(encmask, encmask, 0)  't2 := trailing edge time.
    t2 := cnt
    waitpeq(encmask, encmask, 0)  't3 := next leading edge time.
    t3 := cnt
    if (long[vars][POSD] > 0)     'Comput current position, depending on positve direction of rotation.
      long[vars][CURR] := ((1_000 * (t2 - t1) / (t3 - t1)- 29) * 1000 / 941) #> 0 <# 999
    else
      long[vars][CURR] := 999 - ((1_000 * (t2 - t1) / (t3 - t1)- 29) * 1000 / 941) #> 0 <# 999
    case long[vars][STAT]         'Do task depending on current status.
      IDLE:                       'IDLE: Set speed to zero: no power to motor.
        speed := 0
      NEWTARG:                    'NEWTARG: Get new speed and target position. Change status to MOVING.
        target := long[vars][TARG]
        maxspd := long[vars][MAXSP]
        long[vars][STAT] := MOVING
      MOVING, HOLDING:            'MOVING, HOLDING: Actuate motor depending on position error.
        cerror := (long[vars][CURR] - target) 'Current error := currentposition - target position.
        if cerror < -500                      'Find error to closest (CW or CCW) target.
          cerror += 1000
        elseif cerror > 500
          cerror -= 1000                    
        speed := cerror * long[vars][POSD] / 2 #> -maxspd <# maxspd  'Compute motor speed, limited by max speed.
        splimit := ((cerror ^ perror) ~> 31 & 20)  'Compute backlash compensation.
        if (long[vars][CURR] == pangle)            'If angle hasn't changed, boost the power to avoid stiction.
          splimit += 20
        if (speed < 0)                             'Apply low speed limit.
          speed <#= -splimit
        elseif (speed > 0)
          speed #>= splimit
        if (||error < 2)                           'Set status to HOLDING if within one of target.
          long[vars][STAT] := HOLDING
        long[vars][ERR] := perror := cerror        'Update previous error with current error and write to hub.
    width := _1us * (1520 - (40 & (speed < 0)) - (20 & (speed == 0)) + speed) 'Compute pulse width for servo. 
    delay := _5ms - width                           'Subtract width from 5 ms for off time.
    waitcnt(t0 += delay)                           'Do servo pulse.
    outa[servo_pin]~~
    waitcnt(t0 += width)
    outa[servo_pin]~