-- Aim and Follow Dediblade (Non-Tiltrotor version) PID by /u/wrigh516 1/31/2020 --

-- No need for any control AI cards, will hover and aim at mainframe target --

-- Change Basic Settings for altitude and target distance hold --

-- Turn off AutoCal and manually set PID values in Stability Settings for the best results --

-- Basic Settings --

aimAltitude = 50

aimDistance = 1000

-- Initializing Variables --

aimPitch = 0

aimRoll = 0

aimYaw = 0

distanceError = 0

Spinners = {}

Speed = {}

Speed.Integral = 0

Speed.Error = 0

Speed.Errors = {}

Speed.EIndex = 0

Distance = {}

Distance.Integral = 0

Distance.Error = 0

Distance.Errors = {}

Distance.EIndex = 0

Altitude = {}

Altitude.Integral = 0

Altitude.Error = 0

Altitude.Errors = {}

Altitude.EIndex = 0

Roll = {}

Roll.Integral = 0

Roll.Error = 0

Roll.Errors = {}

Roll.EIndex = 0

Pitch = {}

Pitch.Integral = 0

Pitch.Error = 0

Pitch.Errors = {}

Pitch.EIndex = 0

Yaw = {}

Yaw.Integral = 0

Yaw.Error = 0

Yaw.Errors = {}

Yaw.EIndex = 0

-- Stability Settings --

Speed.P = .1

Speed.I = .1

Speed.Time = 30

Speed.D = 2

Speed.AutoCal = false

Speed.Init = Speed.P

Distance.P = .08

Distance.I = .08

Distance.Time = 30

Distance.D = 1

Distance.AutoCal = false

Distance.Init = Distance.P

Altitude.P = .5

Altitude.I = .5

Altitude.Time = 60

Altitude.D = 15

Altitude.AutoCal = true

Altitude.Init = Altitude.P

Roll.P = .15

Roll.I = .15

Roll.Time = 60

Roll.AutoCal = true

Roll.D = 2

Roll.Init = Roll.P

Pitch.P = .15

Pitch.I = .15

Pitch.Time = 60

Pitch.D = 2

Pitch.AutoCal = true

Pitch.Init = Pitch.P

Yaw.P = .1

Yaw.I = .1

Yaw.Time = 40

Yaw.D = 10

Yaw.AutoCal = false

Yaw.Init = Yaw.P

function Update(I)

-- Calculate Spinner Indexes and Positions --

if (IsSpinnerUpdateRequired(I)) then

SetSpinnersPositions(I)

end

-- Combat AI --

SetControl(I)

-- Set Spinner Angles and Speeds --

SetSpinners(I)

end

-- Internal Functions --

function IsSpinnerUpdateRequired(I)

if (I:GetSpinnerCount() > 1) then

return I:GetSpinnerCount() - 1 ~= table.getn(Spinners)

else

return true

end

end

function SetSpinnersPositions(I)

Spinners = {}

for s = 0, I:GetSpinnerCount() - 1 do

local R = I:GetSpinnerInfo(s).LocalRotation

Spinners[s] = {}

Spinners[s].forward = IsForward(I, I:GetSpinnerInfo(s).Position)

Spinners[s].starboard = IsStarboard(I, I:GetSpinnerInfo(s).Position)

Spinners[s].vertical = (R.w == 0 and R.x == 0 and R.y == 1 and R.z == 0) or (R.w == 1 and R.x == 0 and R.y == 0 and R.z == 0) or (R.w > .70709 and R.w < .70711 and R.x == 0 and R.y < -.70709 and R.y > -.70711 and R.z == 0) or (R.w > .70709 and R.w < .70711 and R.x == 0 and R.y > .70709 and R.y < .70711 and R.z == 0)

Spinners[s].inverted = (R.w == 0 and R.x == 0 and R.y == 0 and R.z == 1) or (R.w == 0 and R.x == 1 and R.y == 0 and R.z == 0) or (R.w == 0 and R.x > .70709 and R.x < .70711 and R.y == 0 and R.z > .70709 and R.z < .70711) or (R.w == 0 and R.x > .70709 and R.x < .70711 and R.y == 0 and R.z < -.70709 and R.z > -.70711)

Spinners[s].pulling = (R.w == .5 and R.x == .5 and R.y == .5 and R.z == .5) or (R.w == .5 and R.x == .5 and R.y == -.5 and R.z == -.5) or (R.w > .70709 and R.w < .70711 and R.x > .70709 and R.x < .70711 and R.y == 0 and R.z == 0) or (R.w == 0 and R.x == 0 and R.y > .70709 and R.y < .70711 and R.z > .70709 and R.z < .70711)

Spinners[s].pushing = (R.w == -.5 and R.x == .5 and R.y == -.5 and R.z == .5) or (R.w == -.5 and R.x == .5 and R.y == .5 and R.z == -.5) or (R.w == 0 and R.x == 0 and R.y > .70709 and R.y < .70711 and R.z < -.70709 and R.z > -.70711) or (R.w > .70709 and R.w < .70711 and R.x < -.70709 and R.x > -.70711 and R.y == 0 and R.z == 0)

I:SetSpinnerPowerDrive(s,10)

end

end

function IsForward(I, Position)

U = I:GetConstructForwardVector()

V = GetLocalPosition(I, Position)

return U.x*V.x + U.y*V.y + U.z*V.z > 0

end

function IsStarboard(I, Position)

U = I:GetConstructRightVector()

V = GetLocalPosition(I, Position)

return U.x*V.x + U.y*V.y + U.z*V.z > 0

end

function GetLocalPosition(I, Position)

local LocalPosition = {}

local CoM = I:GetConstructCenterOfMass()

LocalPosition.x = Position.x - CoM.x

LocalPosition.y = Position.y - CoM.y

LocalPosition.z = Position.z - CoM.z

return LocalPosition

end

function SetControl(I)

if (I:GetNumberOfTargets(0) > 0) then

speed = PID(I, Distance, aimDistance - GetDistance(I:GetConstructPosition(), I:GetTargetPositionInfo(0,0).Position))

else

speed = PID(I, Speed, GetForwardsVelocity(I))

end

if (speed > 25) then

speed = 25

elseif (speed < -25) then

speed = -25

end

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

roll = PID(I, Roll, GetError(I:GetConstructRoll() - GetSidewaysVelocity(I)/2) - aimRoll)

yaw = PID(I, Yaw, -GetError(I:GetTargetPositionInfo(0,0).Azimuth))

altitude = PID(I, Altitude, aimAltitude - I:GetConstructPosition().y)

if (altitude > 25) then

altitude = 25

elseif (altitude < -25) then

altitude = -25

end

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

-- Dampening Calibration Memory --

if (X.AutoCal) then

X.Errors[X.EIndex] = X.Error - lastError

X.EIndex = X.EIndex + 1

AutoCalibrate(I, X)

end

return correction

end

function GetError(angle)

if (angle > 180) then

return -(360 - angle)

else

return angle

end

end

function GetForwardsVelocity(I)

U = I:GetConstructForwardVector()

V = I:GetVelocityVector()

return U.x*V.x + U.y*V.y + U.z*V.z

end

function GetSidewaysVelocity(I)

U = I:GetConstructRightVector()

V = I:GetVelocityVector()

return U.x*V.x + U.y*V.y + U.z*V.z

end

function SetSpinners(I)

for s = 0, I:GetSpinnerCount() - 1 do

if (Spinners[s].pulling) then

I:SetSpinnerContinuousSpeed(s, HorizontalSpeed(Spinners[s]))

elseif (Spinners[s].pushing) then

I:SetSpinnerContinuousSpeed(s, -HorizontalSpeed(Spinners[s]))

elseif (Spinners[s].vertical) then

I:SetSpinnerContinuousSpeed(s, VerticalSpeed(Spinners[s]))

elseif (Spinners[s].inverted) then

I:SetSpinnerContinuousSpeed(s, -VerticalSpeed(Spinners[s]))

end

end

end

function HorizontalSpeed(Spinner)

if (Spinner.starboard) then

return -speed + yaw

else

return -speed - yaw

end

end

function VerticalSpeed(Spinner)

local speed = 0

if (Spinner.forward) then

speed = speed + pitch

else

speed = speed - pitch

end

if (Spinner.starboard) then

speed = speed - roll

else

speed = speed + roll

end

return speed + altitude

end

function AutoCalibrate(I, X)

if (X.EIndex == 14) then

local last = X.Errors[0]

local count = 0

for i = 1, table.getn(X.Errors) - 1 do

if ((last < 0 and X.Errors[i] > 0) or (last > 0 and X.Errors[i] < 0)) then

count = count + 1

end

end

if (count < 2 and X.P < 10*X.Init) then

IncreasePower(X)

else

DecreasePower(X)

end

X.EIndex = 0

end

end

function IncreasePower(X)

X.P = X.P*1.05

X.I = X.I*1.05

X.D = X.D*1.05

end

function DecreasePower(X)

X.P = X.P*.9

X.I = X.I*.9

X.D = X.D*.9

end

function GetDistance(A, B)

return math.sqrt((A.x - B.x)^2 + (A.y - B.y)^2 + (A.z - B.z)^2)

end

function Latitude(A, B)

return -math.atan((A.y - B.y)/(math.sqrt((A.x - B.x)^2 + (A.z - B.z)^2)))*360/(math.pi*2)