PIDController

--[[

	PID Controller for Lua
	Adapted from http://www.computercraft.info/forums2/index.php?/topic/27755-porting-a-pid-controller-to-lua/
	Which was 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:SetInput(Input)
	self.myInput = Input
end

function PID:SetPoint(Setpoint)
	self.mySetpoint = Setpoint
end

function PID:SetOutput(Output)
	self.myOutput = Output
end

function PID:SetIterm(Iterm)
	self.ITerm = Iterm
end


function PID:new(Kp, Ki, Kd, ControllerDirection)
	local obj = {}
	setmetatable(obj, self)
	self.__index = self

	self.kp = Kp
	self.ki = Ki
	self.kd = Kd

	self.inAuto = false

	self:SetOutputLimits(0, 255)

	self.SampleTime = 10
	self:SetControllerDirection(ControllerDirection)

	self:SetTunings(Kp, Ki, Kd)
	self.lastTime = millis() - self.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

		-- print("ki: ", self.ki)
		-- print("kp: ", self.kp)
		-- print("kd: ", self.kd)
		-- print("er: ", errVal)
		-- print("Max: ", self.outMax)
		-- print("Min: ", self.outMin)
		self.ITerm = self.ITerm + (self.ki * errVal)
		-- print("iterm: ", self.ITerm)
		if self.ITerm > self.outMax then self.ITerm = self.outMax end
		if self.ITerm < self.outMin then self.ITerm = self.outMin
		end

		local dInput = input - self.lastInput
		-- print("Last input:", self.lastInput)
		local output = self.kp * errVal + self.ITerm - self.kd * dInput
		-- print("Out: ", output)
		-- output = output

		if output >= self.outMax then output = self.outMax
		elseif output <= self.outMin then output = self.outMin
		end
		-- print("Out: ", output)

		self.myOutput = output
		-- print(self.outMax)
		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:GetOutput()
	return self.myOutput
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