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

local function clamp(value, minValue, maxValue)
    return math.max(minValue, math.min(maxValue, value))
end

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

        local function getSwivelAngle()
            if swivel.getAngle then
                return swivel.getAngle()
            end
            return swivel.getTargetAngle()
        end

        local startAngle = getSwivelAngle()

        while true do
            local angle = getSwivelAngle()
            local wantedAngle = clamp(startAngle + TARGET_ANGLE, startAngle - 45, startAngle + 45)
            local raw = pid:compute(wantedAngle, 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)

                    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 = curX - targetX,
                                        z = curZ - targetZ
                                    }

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

                                    if length > 0 then
                                        local normilized = {
                                            x = direction.x / length,
                                            z = direction.z / length
                                        }

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

                                        local raw = art - yaw
                                        local error = raw % 360
                                        if error > 180 then
                                            error = error - 360
                                        end

                                        TARGET_ANGLE = clamp(error, -45, 45)

                                        print(
                                            string.format(
                                                "curX=%.1f curZ=%.1f yaw=%.1f art=%.1f err=%.1f",
                                                curX,
                                                curZ,
                                                yaw,
                                                art,
                                                error
                                            )
                                        )
                                    else
                                        TARGET_ANGLE = 0
                                    end

                                    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 = clamp(value, -45, 45)
                            elseif param == "h" or param == "height" then
                                TARGET_HEIGHT = value
                            end
                        end
                    end
                end
            end
        end
    end
)