137 lines
3.3 KiB
Lua
137 lines
3.3 KiB
Lua
--:Minify:--
|
|
|
|
local pid = {}
|
|
pid.__index = pid
|
|
|
|
local function clamp(value, minimum, maximum)
|
|
if minimum and value < minimum then
|
|
return minimum
|
|
end
|
|
if maximum and value > maximum then
|
|
return maximum
|
|
end
|
|
return value
|
|
end
|
|
|
|
local function assertNumber(value, name)
|
|
if type(value) ~= "number" then
|
|
error("pid: " .. name .. " must be a number")
|
|
end
|
|
return value
|
|
end
|
|
|
|
function pid.new(kp, ki, kd, options)
|
|
assertNumber(kp, "kp")
|
|
assertNumber(ki, "ki")
|
|
assertNumber(kd, "kd")
|
|
|
|
options = options or {}
|
|
|
|
local controller = setmetatable({
|
|
kp = kp,
|
|
ki = ki,
|
|
kd = kd,
|
|
setpoint = options.setpoint or 0,
|
|
outputMin = options.outputMin,
|
|
outputMax = options.outputMax,
|
|
integralMin = options.integralMin,
|
|
integralMax = options.integralMax,
|
|
lastError = 0,
|
|
integral = 0,
|
|
lastMeasurement = nil,
|
|
lastOutput = 0,
|
|
lastTime = nil,
|
|
}, pid)
|
|
|
|
return controller
|
|
end
|
|
|
|
function pid:reset()
|
|
self.lastError = 0
|
|
self.integral = 0
|
|
self.lastMeasurement = nil
|
|
self.lastOutput = 0
|
|
self.lastTime = nil
|
|
end
|
|
|
|
function pid:setSetpoint(setpoint)
|
|
assertNumber(setpoint, "setpoint")
|
|
self.setpoint = setpoint
|
|
end
|
|
|
|
function pid:setCoefficients(kp, ki, kd)
|
|
assertNumber(kp, "kp")
|
|
assertNumber(ki, "ki")
|
|
assertNumber(kd, "kd")
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.kd = kd
|
|
end
|
|
|
|
function pid:setOutputLimits(minimum, maximum)
|
|
if minimum ~= nil then assertNumber(minimum, "outputMin") end
|
|
if maximum ~= nil then assertNumber(maximum, "outputMax") end
|
|
if minimum and maximum and minimum > maximum then
|
|
error("pid: outputMin must be <= outputMax")
|
|
end
|
|
self.outputMin = minimum
|
|
self.outputMax = maximum
|
|
end
|
|
|
|
function pid:setIntegralLimits(minimum, maximum)
|
|
if minimum ~= nil then assertNumber(minimum, "integralMin") end
|
|
if maximum ~= nil then assertNumber(maximum, "integralMax") end
|
|
if minimum and maximum and minimum > maximum then
|
|
error("pid: integralMin must be <= integralMax")
|
|
end
|
|
self.integralMin = minimum
|
|
self.integralMax = maximum
|
|
end
|
|
|
|
function pid:update(measurement, dt)
|
|
assertNumber(measurement, "measurement")
|
|
assertNumber(dt, "dt")
|
|
if dt <= 0 then
|
|
error("pid: dt must be greater than zero")
|
|
end
|
|
|
|
local errorValue = self.setpoint - measurement
|
|
self.integral = clamp(self.integral + errorValue * dt, self.integralMin, self.integralMax)
|
|
local derivative = 0
|
|
if self.lastTime ~= nil then
|
|
derivative = (errorValue - self.lastError) / dt
|
|
end
|
|
|
|
local output = self.kp * errorValue + self.ki * self.integral + self.kd * derivative
|
|
output = clamp(output, self.outputMin, self.outputMax)
|
|
|
|
self.lastError = errorValue
|
|
self.lastMeasurement = measurement
|
|
self.lastOutput = output
|
|
self.lastTime = (self.lastTime or 0) + dt
|
|
|
|
return output
|
|
end
|
|
|
|
function pid:compute(measurement, dt)
|
|
return self:update(measurement, dt)
|
|
end
|
|
|
|
function pid:getState()
|
|
return {
|
|
setpoint = self.setpoint,
|
|
kp = self.kp,
|
|
ki = self.ki,
|
|
kd = self.kd,
|
|
integral = self.integral,
|
|
lastError = self.lastError,
|
|
lastOutput = self.lastOutput,
|
|
}
|
|
end
|
|
|
|
function pid:resetIntegral()
|
|
self.integral = 0
|
|
end
|
|
|
|
return pid
|