PID controller for Lua/ComputerCraft

PIDController.lua

--[[

	PID Controller for Lua
	Ported from https://github.com/br3ttb/Arduino-PID-Library/

--]]

local function millis()
	return os.clock() * 1000
end

PID = {
	dispKp = 0.0,
	dispKi = 0.0,
	dispKd = 0.0,

	kp = 0.0,
	ki = 0.0,
	kd = 0.0,

	controllerDirection = 0,

	myInput = 0.0,
	myOutput = 0.0,
	mySetpoint = 0.0,

	lastTime = 0,
	ITerm = 0.0,
	lastInput = 0.0,

	SampleTime = 0,
	outMin = 0.0,
	outMax = 0.0,
	inAuto = true
}

-- Constants used by the library
PID.LIBRARY_VERSION = "1.1.1"

PID.AUTOMATIC = 1
PID.MANUAL    = 0
PID.DIRECT    = 0
PID.REVERSE   = 1

-- Common Functions
function PID:new(Input, Output, Setpoint, Kp, Ki, Kd, ControllerDirection)
	local obj = {}
	setmetatable(obj, self)
	self.__index = self

	obj.myOutput = Output
	obj.myInput = Input
	obj.mySetpoint = Setpoint

	obj.inAuto = false

	obj:SetOutputLimits(0, 255)

	obj.SampleTime = 100

	obj:SetControllerDirection(ControllerDirection)
	obj:SetTunings(Kp, Ki, Kd)

	obj.lastTime = millis() - obj.SampleTime

	return obj
end

function PID:SetMode(Mode)
	local newAuto = (Mode == PID.AUTOMATIC)
	if newAuto == not self.inAuto then
		self:Initialize()
	end
	self.inAuto = newAuto
end

function PID:Compute()
	if not self.inAuto then return false end
	local now = millis()
	local timeChange = now - self.lastTime
	if timeChange >= self.SampleTime then
		local input = self.myInput
		local errVal = self.mySetpoint - input
		self.ITerm = self.ITerm + (self.ki * errVal)

		if self.ITerm > self.outMax then self.ITerm = self.outMax
		elseif self.iTerm < self.outMin then self.ITerm = self.outMin
		end

		local dInput = input - self.lastInput

		local output = self.kp * errVal + self.ITerm - self.kd * dInput

		if output > self.outMax then output = self.outMax
		elseif output < self.outMin then output = self.outMin
		end

		self.myOutput = output

		self.lastInput = input
		self.lastTime = now

		return true
	else
		return false
	end
end

function PID:SetOutputLimits(Min, Max)
	if Min >= Max then return end
	self.outMin = Min
	self.outMax = Max

	if self.inAuto then
		if self.myOutput > self.outMax then self.myOutput = self.outMax
		elseif self.myOutput < self.outMin then self.myOutput = self.outMin
		end

		if self.ITerm > self.outMax then self.ITerm = self.outMax
		elseif self.ITerm < self.outMin then self.ITerm = self.outMin
		end
	end
end

-- Less common, but publicly available
function PID:SetTunings(Kp, Ki, Kd)
	if Kp < 0 or Ki < 0 or Kd < 0 then return end

	self.dispKp = Kp
	self.dispKi = Ki
	self.dispKd = Kd

	local SampleTimeInSec = self.SampleTime / 1000.0

	self.kp = Kp
	self.ki = Ki * SampleTimeInSec
	self.kd = Kd / SampleTimeInSec

	if self.ControllerDirection == PID.REVERSE then
		self.kp = 0 - self.kp
		self.ki = 0 - self.ki
		self.kd = 0 - self.kd
	end
end

function PID:SetControllerDirection(Direction)
	if self.inAuto and Direction ~= self.controllerDirection then
		self.kp = 0 - self.kp
		self.ki = 0 - self.ki
		self.kd = 0 - self.kd
	end

	self.controllerDirection = Direction
end

function PID:SetSampleTime(NewSampleTime)
	if NewSampleTime > 0 then
		ratio = NewSampleTime / self.SampleTime

		self.ki = self.ki * ratio
		self.kd = self.kd / ratio

		self.SampleTime = NewSampleTime
	end
end

-- Display functionality
function PID:GetKp()
	return self.dispKp
end

function PID:GetKi()
	return self.dispKi
end

function PID:GetKd()
	return self.dispKd
end

function PID:GetMode()
	if self.inAuto then
		return PID.AUTOMATIC
	else
		return PID.MANUAL
	end
end

function PID:GetDirection()
	return self.controllerDirection
end

-- Private functions
function PID:Initialize()
	self.ITerm = self.myOutput
	self.lastInput = self.myInput

	if self.ITerm > self.outMax then
		self.ITerm = self.outMax
	elseif self.ITerm < self.outMin then
		self.ITerm = self.outMin
	end
end