gistfile1.txt

-- a PI controller with feed-forward
local function PIFF(kFF,kP,kI,iMax)
  -- the new instance
  local self = {}

  -- private fields as locals
  local _kFF = kFF
  local _kP = kP or 0.0
  local _kI = kI or 0.0
  local _kD = kD or 0.0
  local _iMax = iMax
  local _last_t = nil
  local _log_data = {}
  local _I = 0
  local _counter = 0

  -- update controller
  function self.update(target, current)
    local now = millis():tofloat() * 0.001
    if not _last_t then
      _last_t = now
    end
    local dt = now - _last_t
    _last_t = now
    local err = target - current
    _counter = _counter + 1

    local FF = _kFF * target
    local P = _kP * err
    _I = _I + _kI * err * dt
    if _iMax then
       _I = constrain(_I, -_iMax, iMax)
    end
    local I = _I
    local ret = FF + P + I

    _log_data = { target, current, FF, P, I, ret }
    return ret
  end

  function self.log(name)
     logger.write(name,'Targ,Curr,FF,P,I,Total','ffffff',table.unpack(_log_data))
  end

  -- return the instance
  return self
end