-- Simple Hydrofoil PID by /u/wrigh516 1/31/2020 --

-- Will not control roll unless mainframe AI roll control is set to 'Default' in Maneuver settings --

-- Will not control pitch unless mainframe AI pitch control is set to 'Default' in Maneuver settings --

-- If rocking back and forth, change I and D below under Stability Settings --

-- Initializing Variables --

Hydrofoils = {}

Roll = {}

Roll.Integral = 0

Roll.Error = 0

Pitch = {}

Pitch.Integral = 0

Pitch.Error = 0

-- Stability Settings --

Roll.P = 45/3

Roll.I = .3

Roll.Time = 45

Roll.D = 5

Pitch.P = 45/3

Pitch.I = .3

Pitch.Time = 45

Pitch.D = 5

function Update(I)

-- Calculate Indexes and Positions --

if (IsHydrofoilUpdateRequired(I)) then

SetHydrofoilPositions(I)

end

-- Set Spinner Angles and Speeds --

RunPID(I)

SetHydrofoils(I)

end

-- Internal Functions --

function IsHydrofoilUpdateRequired(I)

if (I:Component_GetCount(8) > 1) then

return I:Component_GetCount(8) - 1 ~= table.getn(Hydrofoils)

else

return true

end

end

function SetHydrofoilPositions(I)

Hydrofoils = {}

for h = 0, I:Component_GetCount(8) - 1 do

Hydrofoils[h] = {}

Hydrofoils[h].forward = (I:Component_GetBlockInfo(8,h).LocalPositionRelativeToCom.z > 0)

Hydrofoils[h].starboard = (I:Component_GetBlockInfo(8,h).LocalPositionRelativeToCom.x > 0)

end

end

function RunPID(I)

roll = PID(I, Roll, GetError(I:GetConstructRoll()))

pitch = PID(I, Pitch, GetError(I:GetConstructPitch()))

end

function PID(I, X, error)

local lastError = X.Error

X.Error = error

-- Apply Proportional --

local correction = X.Error*X.P

-- Apply Integral --

X.Integral = X.Integral*(X.Time - 1)/X.Time + correction*(1/X.Time)

correction = correction + X.Integral*X.I

-- Apply Derivative --

correction = correction + (X.Error - lastError)*X.D

return correction

end

function GetError(angle)

if (angle > 180) then

return -(360 - angle)

else

return angle

end

end

function SetHydrofoils(I)

for h = 0, I:Component_GetCount(8) - 1 do

I:Component_SetFloatLogic(8,h,CalculateAngle(I, Hydrofoils[h]))

end

end

function CalculateAngle(I, Hydrofoil)

local angle = 0;

if (Hydrofoil.forward) then

angle = pitch

else

angle = -pitch

end

if (Hydrofoil.starboard) then

angle = angle - roll

else

angle = angle + roll

end

if (angle < -45) then

angle = -45

elseif (angle > 45) then

angle = 45

end

if (I:GetForwardsVelocityMagnitude() > 0) then

return angle

else

return -angle

end