Skip to content

Commit

Permalink
PID delta time
Browse files Browse the repository at this point in the history
  • Loading branch information
Sleitnick committed Jan 16, 2024
1 parent 936cf43 commit 211c038
Showing 1 changed file with 36 additions and 13 deletions.
49 changes: 36 additions & 13 deletions modules/pid/init.lua
Original file line number Diff line number Diff line change
@@ -1,10 +1,9 @@
-- PID
-- August 11, 2020
--!native

export type PID = {
POnE: boolean,
Reset: (self: PID) -> (),
Calculate: (self: PID, setpoint: number, input: number) -> number,
Calculate: (self: PID, setpoint: number, input: number, deltaTime: number) -> number,
Debug: (self: PID, name: string, parent: Instance?) -> (),
Destroy: (self: PID) -> (),
}
Expand Down Expand Up @@ -61,14 +60,19 @@ PID.__index = PID
]=]
function PID.new(min: number, max: number, kp: number, ki: number, kd: number): PID
local self = setmetatable({}, PID)

self._min = min
self._max = max

self._kp = kp
self._ki = ki
self._kd = kd

self._lastInput = 0
self._outputSum = 0

self.POnE = true

return self
end

Expand Down Expand Up @@ -99,10 +103,13 @@ end
end)
```
]=]
function PID:Calculate(setpoint: number, input: number)
local err = (setpoint - input)
local dInput = (input - self._lastInput)
self._outputSum += (self._ki * err)
function PID:Calculate(setpoint: number, input: number, deltaTime: number)
local ki = self._ki * deltaTime
local kd = self._kd * deltaTime

local err = setpoint - input
local dInput = input - self._lastInput
self._outputSum += ki * err

if not self.POnE then
self._outputSum -= self._kp * dInput
Expand All @@ -115,7 +122,7 @@ function PID:Calculate(setpoint: number, input: number)
output = self._kp * err
end

output += self._outputSum - self._kd * dInput
output += self._outputSum - kd * dInput
output = math.clamp(output, self._min, self._max)

self._lastInput = input
Expand All @@ -138,6 +145,7 @@ function PID:Debug(name: string, parent: Instance?)
if not game:GetService("RunService"):IsStudio() then
return
end

if self._debug then
return
end
Expand All @@ -154,11 +162,26 @@ function PID:Debug(name: string, parent: Instance?)
end)
end

Bind("Min", "_min")
Bind("Max", "_max")
Bind("KP", "_kp")
Bind("KI", "_ki")
Bind("KD", "_kd")
folder:SetAttribute("MinMax", NumberRange.new(self._min, self._max))
folder:GetAttributeChangedSignal("MinMax"):Connect(function()
local minMax = folder:GetAttribute("MinMax") :: NumberRange
self._min = minMax.Min
self._max = minMax.Max
self:Reset()
end)

Bind("kP", "_kp")
Bind("kI", "_ki")
Bind("kD", "_kd")

folder:SetAttribute("Output", self._min)

local lastOutput = 0
self.Calculate = function(...)
lastOutput = PID.Calculate(...)
folder:SetAttribute("Output", lastOutput)
return lastOutput
end

folder.Parent = parent or workspace
self._debug = folder
Expand Down

0 comments on commit 211c038

Please sign in to comment.