Motor_ABEncoder3.spin2

'AB Incremental Encoder Driver
CON
  VERSION = 3

VAR
  my_smartpin         'smartpin that gets activated
  my_ticks_per_rev_f    'has to be at least 1.0
  my_prevtick
  my_deltatick

  my_timestamp
  my_prevtimestamp
  my_deltatime

  my_prevspeed_f1
  my_prevspeed_f2
  my_prevspeed_f3

  my_cpu_clock_freq_f

PUB include()
  repeat

PUB start(a_pin, b_pin, ticks_per_rev, cpu_freq) : result | dist
  dist := b_pin - a_pin                                                         'distance between AB pins
  if ((dist == 0) or (abs(dist) > 3))
    result := false
  else
    pinstart(a_pin, P_QUADRATURE | dist.[2..0] << 24, 0, 0)                     'start a/b quadrature mode
    my_smartpin := a_pin
    my_ticks_per_rev_f := float(ticks_per_rev)
    my_cpu_clock_freq_f := float(cpu_freq)

    my_prevtimestamp := getct()
    my_prevtick := rdpin(my_smartpin)
    my_prevspeed_f2 := my_prevspeed_f1 := 0.0

    result := true


pub update() : fspeed | nowtick, delta_sec
  my_timestamp := getct()
  nowtick := rdpin(my_smartpin)
  my_deltatick := nowtick - my_prevtick
  my_deltatime := my_timestamp - my_prevtimestamp

  delta_sec := float(my_deltatime) /. my_cpu_clock_freq_f

  fspeed := FLOAT(my_deltatick) /. (my_ticks_per_rev_f *. delta_sec)
  fspeed := (fspeed +. fspeed +. my_prevspeed_f1 +. my_prevspeed_f2) *. 0.25

  my_prevtick := nowtick
  my_prevtimestamp := my_timestamp
  my_prevspeed_f2 := my_prevspeed_f1
  my_prevspeed_f1 := fspeed