startup.lua

local altitude = peripheral.find("altitude_sensor")

local relay = peripheral.find("redstone_relay")

local speedRotation = peripheral.find("Create_RotationSpeedController")

local swivel = peripheral.find("swivel_bearing")

local PID = require ".pid"



term.clear()

term.setCursorPos(1, 1)



local TARGET_HEIGHT = 100

local TARGET_ANGLE  = 0



parallel.waitForAny(

    -- Module: AutoHeight

    function ()

        local burnerSide = "top"



        local lastHeight = altitude.getHeight()

        local lastTime = os.clock()



        local velocity = 0



        local DEADZONE = 2           -- зона покоя

        local STRONG_ZONE = 40       -- сильный недолёт

        local PREDICT_TIME = 6       -- прогноз вперёд

        local MAX_BURN = 4           -- максимум работы горелки

        local MIN_BURN = 0.15        -- минимальный импульс



        local function burner(state)

            redstone.setOutput(burnerSide, state)

        end



        while true do

            local now = os.clock()

            local dt = now - lastTime

            lastTime = now

            local height = altitude.getHeight()

            velocity = (height - lastHeight) / dt

            lastHeight = height

            local predictedHeight = height + velocity * PREDICT_TIME

            local error = TARGET_HEIGHT - predictedHeight

            if velocity > 3 and height > TARGET_HEIGHT then

                burner(false)

            elseif math.abs(error) <= DEADZONE then

                burner(false)

            elseif error > 0 then



                local burnTime

                if error > STRONG_ZONE then

                    burnTime = MAX_BURN

                else

                    burnTime = math.max(

                        MIN_BURN,

                        math.min(MAX_BURN, error * 0.5)

                    )

                end



                burner(true)

                sleep(burnTime)

                burner(false)

            else

                burner(false)

            end

            sleep(0.05)

        end

    end,



    -- Module: AutoAngle

    function ()

        local pid = PID:new(0.5, 0.001, 0.5, -5, 5)

        local block = false

        while true do

            local angle = swivel.getTargetAngle()

            local raw = pid:compute(TARGET_ANGLE, angle, 1)

            local limited = -math.max(-20, math.min(raw, 20))

            if math.abs(limited) > 0.02 and math.abs(limited) < 1 then

                if block == false then

                    if (limited > 0) then

                        speedRotation.setTargetSpeed(math.ceil(limited))

                    else

                        speedRotation.setTargetSpeed(math.floor(limited))

                    end

                    block = true

                else

                    speedRotation.setTargetSpeed(0)

                end

            else

                if block then block = false end

                speedRotation.setTargetSpeed(limited)

            end

            sleep(0.1)

        end

    end,



    -- Module: Command handler + auto‑навигация

    function ()

        local lastCommand = ""

        while true do

            local cmd = read()

            if cmd and cmd ~= "" and cmd ~= lastCommand then

                lastCommand = cmd

                local colonPos = string.find(cmd, ":")

                if colonPos then

                    local param = string.sub(cmd, 1, colonPos-1):lower()

                    local valueStr = string.sub(cmd, colonPos+1)



                    -- Обработка auto:x,z

                    if param == "auto" then

                        local commaPos = string.find(valueStr, ",")

                        if commaPos then

                            local targetX = tonumber(string.sub(valueStr, 1, commaPos-1))

                            local targetZ = tonumber(string.sub(valueStr, commaPos+1))

                            if targetX and targetZ and sublevel then

                                while true do

                                    local pose = sublevel.getLogicalPose()

                                    local curX = pose.position.x

                                    local curZ = pose.position.z

                                    local w = pose.orientation.a

                                    local x = pose.orientation.v.x

                                    local y = pose.orientation.v.y

                                    local z = pose.orientation.v.z



                                    local siny_cosp = 2 * (w*y + x*z)

                                    local cosy_cosp = 1 - 2 * (y*y + z*z)

                                    local yaw = math.deg(math.atan(siny_cosp, cosy_cosp))



                                    local direction = { x = targetX - curX, z = targetZ - curZ }

                                    local length = math.sqrt(direction.x^2+direction.z^2)

                                    local normilized = { x = direction.x/length, z = direction.z/length }

                                    local art = math.atan(normilized.x, normilized.z) * (180 / math.pi)

                                    local error = yaw - art



                                    TARGET_ANGLE = error * 2

                                    print(string.format("art=%.1f, yaw=%.1f, TARGET_ANGLE=%.1f", art, yaw, TARGET_ANGLE))

                                    sleep(0.05)

                                end

                            else

                                print("Error")

                            end

                        end

                    else

                        local value = tonumber(valueStr)

                        if value then

                            if param == "a" or param == "angle" then

                                TARGET_ANGLE = value

                            elseif param == "h" or param == "height" then

                                TARGET_HEIGHT = value

                            end

                        end

                    end

                end

            end

        end

    end

)