Drone V2.0

Good Drone, need code for fly
H - Hover Mode
WASD - Move
Shift - down
Space - up
Q,E - Rotate
Code:

-- Инициализация датчиков, пульта и мониторов
local alt_front  = peripheral.wrap("altitude_sensor_0")
local alt_back   = peripheral.wrap("altitude_sensor_1")
local alt_left   = peripheral.wrap("altitude_sensor_2")
local alt_right  = peripheral.wrap("altitude_sensor_3")
local typewriter = peripheral.wrap("linked_typewriter_0")
local monitor    = peripheral.wrap("monitor_0")
local monitor_ah = peripheral.wrap("monitor_1")


if not (alt_front and alt_back and alt_left and alt_right and typewriter) then
    print("ERROR: Not all sensors or linked_typewriter_0 found!")
    return
end


if monitor then monitor.setTextScale(1.0) end


-- ================= НАСТРОЙКИ ПОЛЕТА =================
local CLUTCH_SIDE = "right"


local TILT_AMOUNT = 2.5
local YAW_SPEED   = 150
local LIFT_POWER  = 25  


-- Коэффициенты ПИД (Крен, Тангаж, Высота)
local Kp_roll = 1.2; local Ki_roll = 0.03; local Kd_roll = 8.0
local Kp_pitch = 1.5; local Ki_pitch = 0.05; local Kd_pitch = 15.0  
local Kp_alt = 2.0; local Ki_alt = 0.05; local Kd_alt = 10.0


local TRIM_PITCH = 10.0; local TRIM_ROLL  = 0.0  
local HOVER_SPEED = 45; local MIN_THROTTLE = 15; local MAX_SPEED = 256


-- Базовые переменные
local current_throttle = 0; local throttle_step = 0.5  
local prev_err_x, prev_err_z, prev_err_y = 0, 0, 0
local int_x, int_z, int_y = 0, 0, 0


-- Переменные Автопилота
local hover_mode = false
local autoland_mode = false
local target_altitude = 0
local prev_avg_alt = 0
local autoland_throttle = 0 -- НОВАЯ ПЕРЕМЕННАЯ ДЛЯ ПЛАВНОЙ ПОСАДКИ
local prev_keys = {}


-- ================= ФУНКЦИЯ ПАМЯТИ =================
local SAVE_FILE = "flight_data.txt"
local save_timer = os.clock()


local function saveState()
    local f = fs.open(SAVE_FILE, "w")
    if f then
        f.writeLine(tostring(current_throttle))
        f.writeLine(tostring(int_x))
        f.writeLine(tostring(int_z))
        f.close()
    end
end


local function loadState()
    if fs.exists(SAVE_FILE) then
        local f = fs.open(SAVE_FILE, "r")
        if f then
            local t = tonumber(f.readLine())
            local ix = tonumber(f.readLine())
            local iz = tonumber(f.readLine())
            if t and ix and iz then
                current_throttle = t; int_x = ix; int_z = iz
            end
            f.close()
        end
    end
end


local function resetState()
    current_throttle = 0; int_x = 0; int_z = 0; int_y = 0
    hover_mode = false; autoland_mode = false; autoland_throttle = 0
    saveState()
end


loadState()


-- ================= ВСПОМОГАТЕЛЬНЫЕ ФУНКЦИИ =================
local function clamp(v, min, max)
    return v < min and min or (v > max and max or v)
end


local function isPressed(keys_table, code)
    if not keys_table then return false end
    for _, k in ipairs(keys_table) do
        if k == code then return true end
    end
    return false
end


local function justPressed(keys_now, keys_prev, code)
    return isPressed(keys_now, code) and not isPressed(keys_prev, code)
end


local function osd_print(text)
    if monitor then
        local x, y = monitor.getCursorPos()
        monitor.write(text)
        monitor.setCursorPos(1, y + 1)
    else print(text) end
end


local function draw_artificial_horizon(mon, pitch_err, roll_err, alt)
    if not mon then return end
    mon.setTextScale(0.5)
    local w, h = mon.getSize()
    local cx = math.floor(w / 2); local cy = math.floor(h / 2)
    local pitch_offset = math.floor(-pitch_err * 2.0)
    local roll_slope = roll_err * 0.4


    for y = 1, h do
        local text_row, fg_row, bg_row = "", "", ""
        for x = 1, w do
            local horizon_y = cy + pitch_offset + (x - cx) * roll_slope
            if y < horizon_y - 0.5 then bg_row = bg_row .. "3"
            elseif math.abs(y - horizon_y) <= 0.5 then bg_row = bg_row .. "0"
            else bg_row = bg_row .. "c" end
            text_row = text_row .. " "; fg_row = fg_row .. "f"
        end
        mon.setCursorPos(1, y); mon.blit(text_row, fg_row, bg_row)
    end
    mon.setCursorPos(cx - 3, cy); mon.blit(" >-|-< ", "4444444", "7777777")
    mon.setCursorPos(2, 2); mon.setBackgroundColor(colors.black); mon.setTextColor(colors.white)
    mon.write(string.format(" ALT: %.1f ", alt))
    mon.setCursorPos(2, h - 1)
    mon.write(string.format(" P: %0.1f | R: %0.1f ", -pitch_err, -roll_err))
end


-- ================= ГЛАВНЫЙ ЦИКЛ =================
while true do
    -- 1. Читаем высотомеры
    local ok_f, h_front = pcall(alt_front.getHeight)
    local ok_b, h_back  = pcall(alt_back.getHeight)
    local ok_l, h_left  = pcall(alt_left.getHeight)
    local ok_r, h_right = pcall(alt_right.getHeight)
   
    local err_x, err_z, avg_alt = 0, 0, 0
    if ok_f and ok_b and ok_l and ok_r then
        avg_alt = (h_front + h_back + h_left + h_right) / 4.0
    end


    local vertical_speed = (avg_alt - prev_avg_alt) / 0.05
    prev_avg_alt = avg_alt


    -- 2. Считываем пульт
    local keys = typewriter.getPressedKeyCodes() or {}
    local target_x, target_z, manual_throttle, yaw_cmd = 0, 0, 0, 0
    local is_turning = false
    local input_str = ""


    -- Переключатели режимов
    if justPressed(keys, prev_keys, 72) then
        hover_mode = not hover_mode
        if hover_mode then
            autoland_mode = false; target_altitude = avg_alt; int_y = 0
        end
    end


    if justPressed(keys, prev_keys, 76) then
        autoland_mode = not autoland_mode
        if autoland_mode then
            hover_mode = false
            autoland_throttle = 0 -- Сбрасываем счетчик давления газа при включении посадки
        end
    end


    -- ================= ЛОГИКА АВТОПОСАДКИ =================
    if autoland_mode then
        input_str = "[ AUTO-LAND ACTIVE ] "
        -- Каждые 0.05 сек дрон давит газ всё ниже и ниже (по -0.2 RPM за тик)
        autoland_throttle = autoland_throttle - 0.2
        manual_throttle = autoland_throttle


        -- КАК ПОНЯТЬ, ЧТО МЫ СЕЛИ:
        -- 1. Высота почти ноль (если датчики на пузе)
        -- 2. ИЛИ мы скинули газ уже очень сильно (-20 RPM), а дрон не двигается вниз (уперлись в землю)
        if avg_alt < 0.5 or (autoland_throttle < -20 and math.abs(vertical_speed) < 0.2) then
            resetState()
            peripheral.call("Create_RotationSpeedController_6", "setTargetSpeed", 0)
        end
    else
        -- Обычное управление
        if isPressed(keys, 87) then target_z = -TILT_AMOUNT; input_str = input_str .. "[W] " end              
        if isPressed(keys, 83) then target_z = TILT_AMOUNT; input_str = input_str .. "[S] " end              
        if isPressed(keys, 65) then target_x = -TILT_AMOUNT; input_str = input_str .. "[A] " end              
        if isPressed(keys, 68) then target_x = TILT_AMOUNT; input_str = input_str .. "[D] " end              
        if isPressed(keys, 81) then yaw_cmd = YAW_SPEED; is_turning = true; input_str = input_str .. "[Q] " end                
        if isPressed(keys, 69) then yaw_cmd = -YAW_SPEED; is_turning = true; input_str = input_str .. "[E] " end                  


        if hover_mode then
            input_str = "[HOVER] " .. input_str
            if isPressed(keys, 32) then target_altitude = target_altitude + 0.3; input_str = input_str .. "[UP] " end
            if isPressed(keys, 340) then target_altitude = target_altitude - 0.3; input_str = input_str .. "[DN] " end
        else
            if isPressed(keys, 32) then manual_throttle = LIFT_POWER; input_str = input_str .. "[SPC] " end      
            if isPressed(keys, 340) then manual_throttle = -LIFT_POWER; input_str = input_str .. "[SHF] " end    
        end
    end


    if input_str == "" then input_str = "IDLE" end
    prev_keys = keys


    -- УПРАВЛЕНИЕ СЦЕПЛЕНИЕМ
    if is_turning and not autoland_mode then
        redstone.setOutput(CLUTCH_SIDE, false)
        peripheral.call("Create_RotationSpeedController_6", "setTargetSpeed", yaw_cmd)
    else
        redstone.setOutput(CLUTCH_SIDE, true)  
    end


    -- 3. ПИД ВЫСОТЫ (Hover Mode)
    local pid_throttle = 0
    if hover_mode and not autoland_mode then
        local err_y = target_altitude - avg_alt
        int_y = clamp(int_y + err_y, -500, 500)
        pid_throttle = (Kp_alt * err_y) + (Ki_alt * int_y) + (Kd_alt * (err_y - prev_err_y))
        prev_err_y = err_y
    else
        int_y = 0; prev_err_y = 0
    end


    -- 4. Расчет газа
    local alt_cmd = -peripheral.call("Create_RotationSpeedController_5", "getTargetSpeed")
    local target_throttle = HOVER_SPEED + alt_cmd
   
    if hover_mode then target_throttle = target_throttle + pid_throttle
    else target_throttle = target_throttle + manual_throttle end
   
    target_throttle = clamp(target_throttle, MIN_THROTTLE, MAX_SPEED)


    if current_throttle < target_throttle then
        current_throttle = math.min(current_throttle + throttle_step, target_throttle)
    elseif current_throttle > target_throttle then
        current_throttle = math.max(current_throttle - (throttle_step * 2), target_throttle)
    end
    local throttle = current_throttle


    -- 5. ПИД Наклона
    err_z = target_z - (h_front - h_back)
    err_x = target_x - (h_left - h_right)


    if math.abs(err_x) < 0.1 and target_x == 0 then err_x = 0 end
    if math.abs(err_z) < 0.1 and target_z == 0 then err_z = 0 end


    if throttle <= MIN_THROTTLE + 5 then
        int_x = 0; int_z = 0
    else
        int_x = clamp(int_x + err_x, -500, 500)
        int_z = clamp(int_z + err_z, -500, 500)
    end


    local output_x = (Kp_roll * err_x) + (Ki_roll * int_x) + (Kd_roll * (err_x - prev_err_x)) - TRIM_ROLL
    local output_z = (Kp_pitch * err_z) + (Ki_pitch * int_z) + (Kd_pitch * (err_z - prev_err_z)) - TRIM_PITCH


    prev_err_x = err_x; prev_err_z = err_z


    -- Микшер
    local motor_1 = throttle + output_z + output_x
    local motor_2 = throttle + output_z - output_x
    local motor_4 = throttle - output_z + output_x
    local motor_3 = throttle - output_z - output_x


    motor_1 = clamp(motor_1, MIN_THROTTLE, MAX_SPEED); motor_2 = clamp(motor_2, MIN_THROTTLE, MAX_SPEED)
    motor_4 = clamp(motor_4, MIN_THROTTLE, MAX_SPEED); motor_3 = clamp(motor_3, MIN_THROTTLE, MAX_SPEED)


    peripheral.call("Create_RotationSpeedController_1", "setTargetSpeed", -motor_1)
    peripheral.call("Create_RotationSpeedController_2", "setTargetSpeed", -motor_2)
    peripheral.call("Create_RotationSpeedController_4", "setTargetSpeed", -motor_4)
    peripheral.call("Create_RotationSpeedController_3", "setTargetSpeed", -motor_3)


    -- === СИГНАЛИЗАЦИЯ GPWS ===
    local alarm_msg = nil
    if vertical_speed < -5.0 and avg_alt > 3.0 then
        alarm_msg = "!! PULL UP !!"
    elseif math.abs(err_x) > 6.0 or math.abs(err_z) > 6.0 then
        alarm_msg = "!! BANK ANGLE !!"
    end


    -- === ОБНОВЛЕНИЕ ГЛАВНОГО ЭКРАНА ===
    if monitor then
        monitor.clear(); monitor.setCursorPos(1,1)
        monitor.write("=== AERONAUTICS FLIGHT OS ===")
        monitor.setCursorPos(1,2)
        if hover_mode then monitor.write(string.format("PWR: %.1f | ALT: %.1f (Trgt: %.1f)", throttle, avg_alt, target_altitude))
        else monitor.write(string.format("PWR: %.1f | ALT: %.1f", throttle, avg_alt)) end
        monitor.setCursorPos(1,3); monitor.write("INPUT: " .. input_str)
        monitor.setCursorPos(1,4); monitor.write(string.format("PITCH: Trgt %4.1f | Err %5.2f", target_z, err_z))
        monitor.setCursorPos(1,5); monitor.write(string.format("ROLL : Trgt %4.1f | Err %5.2f", target_x, err_x))
       
        if alarm_msg then
            monitor.setCursorPos(1, 7)
            if math.floor(os.clock() * 6) % 2 == 0 then
                monitor.setBackgroundColor(colors.red); monitor.setTextColor(colors.white)
            else
                monitor.setBackgroundColor(colors.black); monitor.setTextColor(colors.red)
            end
            monitor.write(" " .. alarm_msg .. " ")
            monitor.setBackgroundColor(colors.black); monitor.setTextColor(colors.white)
        end


        monitor.setCursorPos(15, 10)
        monitor.setBackgroundColor(colors.red); monitor.setTextColor(colors.white)
        monitor.write("[ RESET ]")
        monitor.setBackgroundColor(colors.black)
    end


    if monitor_ah then draw_artificial_horizon(monitor_ah, err_z, err_x, avg_alt) end


    -- Сохранение состояния
    if os.clock() - save_timer > 1.0 then
        saveState(); save_timer = os.clock()
    end


    -- Ожидание и проверка тачскрина
    local timer = os.startTimer(0.05)
    while true do
        local event, p1, p2, p3 = os.pullEvent()
        if event == "monitor_touch" and p1 == "monitor_0" then
            if p2 >= 15 and p3 >= 10 then
                resetState()
                peripheral.call("Create_RotationSpeedController_6", "setTargetSpeed", 0)
            end
        elseif event == "timer" and p1 == timer then
            break
        end
    end
end