Quant

pid_loop

Aug 3rd, 2016
158
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. local r={}
  2.  r.add=function(get,set,kp,ki,kd,min,max)
  3.  --начало создания
  4.  local loop={}
  5.  loop.get=get
  6.  loop.set=set
  7.  loop.kp=kp
  8.  loop.ki=ki
  9.  loop.kd=kd
  10.  --начальные значения
  11.  loop.reset=function(loop)
  12.  loop.p0=0
  13.  loop.I=0
  14.  loop.D=0
  15.  end
  16.  loop:reset()
  17.  --функции
  18.  loop.execute=function(loop,interval)
  19.   local get=get()
  20.    if interval>0 then --на всякий случай(позволяет пихать значение в функцию без предварительной обработки)
  21.    loop.I=loop.I+get*interval     --интегральная часть
  22.    loop.D=(get-loop.p0)/interval  --дифференциальная часть
  23.    loop.p0=get
  24.    end
  25.   local sum=loop.kp*get+loop.ki*loop.I+loop.kd*loop.D
  26.    if min then sum=math.max(min,sum) end
  27.    if max then sum=math.min(max,sum) end
  28.    set(sum)
  29.   end
  30.   return loop
  31.  end
  32. return r
RAW Paste Data