Drone autopilot [Sable]

local data = {}
local destination = {x=0, z=0}
local tiltAngle = 10 -- angle the drone will tilt towards objective
local triggerangle = 2 -- margin of angle tilt to consider the drone, stable

function TiltTowards()
    local location = data.position
    local velocity = sublevel.getVelocity()

    local angleY = data.orientation.v.y * 180
    local angleX = data.orientation.v.x * 180 * angleY/180
    local angleZ = data.orientation.v.z * 180 * angleY/180

    --Face destination
    if (math.abs(location.x-destination.x) < 200 and math.abs(location.z-destination.z) < 200) then
        tiltAngle = 3
    end

    local enginePower = 0
    if (location.y > -20) then

        if (location.x > destination.x) then
            angleX = angleX+tiltAngle
        elseif (location.x < destination.x) then
            angleX = angleX-tiltAngle
        end

        if (location.z > destination.z) then
            angleZ = angleZ+tiltAngle
        elseif (location.z < destination.z) then
            angleZ = angleZ-tiltAngle
        end

    end

    --Landing
    if (math.abs(location.x - destination.x) < 100 and math.abs(location.z - destination.z) < 100) then
        enginePower = 10
    end

    -- X stabilization
    if (angleX >= triggerangle) then
        redstone.setAnalogOutput("back", enginePower+1)
    else
        redstone.setAnalogOutput("back", enginePower)
    end

    if (angleX <= -triggerangle) then
        redstone.setAnalogOutput("front", enginePower+1)
    else
        redstone.setAnalogOutput("front", enginePower)
    end

    -- Y stabilization
    if (angleZ >= triggerangle) then
        redstone.setAnalogOutput("right", enginePower+1)
    else
        redstone.setAnalogOutput("right", enginePower)
    end

    if (angleZ <= -triggerangle) then
        redstone.setAnalogOutput("left", enginePower+1)
    else
        redstone.setAnalogOutput("left", enginePower)
    end
end

function DisplayInfo()
    local location = data.position
    local velocity = sublevel.getVelocity()

    local angleY = data.orientation.v.y * 180
    local angleX = data.orientation.v.x * 180 * angleY/180
    local angleZ = data.orientation.v.z * 180 * angleY/180

    term.setCursorPos(1,1)
    print("X:" .. destination.x .. " Z:" .. destination.z)

    term.setCursorPos(2,4)
    print("Direction / Angle / Speed")
    term.setCursorPos(2,5)
    print("North / " .. angleX .. " / " .. velocity.x)
    term.setCursorPos(2,6)
    print("South / " .. -angleX .. " / " .. -velocity.x)
    term.setCursorPos(2,7)
    print("East / " .. angleZ .. " / " .. velocity.z)
    term.setCursorPos(2,8)
    print("West / " .. -angleZ .. " / " .. -velocity.z)
end

print("Write destination coordinates.")
print("X:")
destination.x = tonumber(read())
print("Z:")
destination.z = tonumber(read())

while (true) do
    data = sublevel.getLogicalPose()
    TiltTowards()
    DisplayInfo()
    sleep(0.1)
end