Untitled

# Библиотеки
import numpy as np

# Константы
R_luna = 1738000  # [м] Луна - сфера радиуса R_m
mu_luna = 4.903 * (10 ** 12)  # м^3/с^2 - гравитационный параметр Луны
m_0 = 3015  # [кг] начальная масса СВ
delta_m_t_max = 1550  # [кг] предельная масса рабочего запаса топлива ДУ СВ
m_const = 615  # [кг] масса конструкции СВ
t_v = 14  # [с] продолжительность вертикального участка выведения
delta_t = 0.1  # [с] шаг интегрирования

# Начальные данные
W_ist = 3420  # [м/с] эффективная скорость истечения топлива
P_1 = 9100  # [Н] тяга СВ 1
P_2 = 10600  # [Н] тяга СВ 2
h_isl1_1 = 125000  # [м]
h_isl1_2 = 200000  # [м]
h_isl2_1 = 200000  # [м]
h_isl2_2 = 355000  # [м]

# Задаем текущую высоту орбиты и тягу
h = h_isl1_1
P = P_1

# н.у.
Vx = 0
Vy = 0
x = 0
y = 0
t = 0

# углы для управления вектором тяги ДУ - радианы
theta_1_pr = -0.005  # radians
theta_2 = -0.05  # radians

# значения конечного времени для 1 и 2  участка
t_1 = 360.738
t_2 = 700.245


# проекции ускорения от грав. поля Луны
def g_x(x, y):
    return - (mu_luna * x) / ((x ** 2 + (R_luna + y) ** 2) ** (3 / 2))


def g_y(x, y):
    return - (mu_luna * (R_luna + y)) / ((x ** 2 + (R_luna + y) ** 2) ** (3 / 2))


# Закон изменения угла тангажа на активных участках
def theta(t, thata_1_pr, theta_2):
    if t <= t_v:
        return np.pi / 2
    elif (t_v < t <= t_1):
        return np.pi / 2 + thata_1_pr * (t - t_v)
    elif (t_2 <= t):
        return theta_2
    else:
        return 0


# мат.модель
def dVx_dt(t, P, m, x, y, theta_1_dot, theta_2):
    return P * np.cos(theta(t, theta_1_dot, theta_2)) / m + g_x(x, y)


def dVy_dt(t, P, m, x, y, theta_1_dot, theta_2):
    return P * np.sin(theta(t, theta_1_dot, theta_2)) / m + g_y(x, y)


def dm_dt(P):
    return - (P / W_ist)

#РунгеКутта4
def RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt):
    # расчет коэффициентов: k1, k2, k3, k4 через \n\n соответственно
    Vx_k1 = dt * dVx_dt(t, P, m, x, y, theta_1_dot, theta_2)
    Vy_k1 = dt * dVy_dt(t, P, m, x, y, theta_1_dot, theta_2)
    x_k1 = dt * Vx
    y_k1 = dt * Vy
    m_k1 = dt * dm_dt(P)

    Vx_k2 = dt * dVx_dt(t + dt / 2, P, m + m_k1 / 2, x + x_k1 / 2, y + y_k1 / 2, theta_1_dot, theta_2)
    Vy_k2 = dt * dVy_dt(t + dt / 2, P, m + m_k1 / 2, x + x_k1 / 2, y + y_k1 / 2, theta_1_dot, theta_2)
    x_k2 = dt * (Vx + Vx_k1 / 2)
    y_k2 = dt * (Vy + Vy_k1 / 2)
    m_k2 = dt * dm_dt(P)

    Vx_k3 = dt * dVx_dt(t + dt / 2, P, m + m_k2 / 2, x + x_k2 / 2, y + y_k2 / 2, theta_1_dot, theta_2)
    Vy_k3 = dt * dVy_dt(t + dt / 2, P, m + m_k2 / 2, x + x_k2 / 2, y + y_k2 / 2, theta_1_dot, theta_2)
    x_k3 = dt * (Vx + Vx_k2 / 2)
    y_k3 = dt * (Vy + Vy_k2 / 2)
    m_k3 = dt * dm_dt(P)

    Vx_k4 = dt * dVx_dt(t + dt, P, m + m_k3, x + x_k3, y + y_k3, theta_1_dot, theta_2)
    Vy_k4 = dt * dVy_dt(t + dt, P, m + m_k3, x + x_k3, y + y_k3, theta_1_dot, theta_2)
    x_k4 = dt * (Vx + Vx_k3)
    y_k4 = dt * (Vy + Vy_k3)
    m_k4 = dt * dm_dt(P)

    # расчет новых величин в конце участка, соответствующего времени t + dt
    Vx += (Vx_k1 + Vx_k2 * 2 + Vx_k3 * 2 + Vx_k4) / 6
    Vy += (Vy_k1 + Vy_k2 * 2 + Vy_k3 * 2 + Vy_k4) / 6
    x += (x_k1 + x_k2 * 2 + x_k3 * 2 + x_k4) / 6
    y += (y_k1 + y_k2 * 2 + y_k3 * 2 + y_k4) / 6
    m += (m_k1 + m_k2 * 2 + m_k3 * 2 + m_k4) / 6

    return Vx, Vy, x, y, m


# Функция основного расчета параметров траектории
def integration(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt, t_1, t_2):

    print('\nДанные выводятся в формате: Vx [м/с], Vy [м/с], x [м], y [м], m [кг], t [с]')

    print('\nРАСЧЕТ ТОЧКИ t_v\n')

    while t < t_v:
        Vx, Vy, x, y, m = RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt)
        t += dt
        if round(t, 1) == t_v:
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))

    print('\nРАСЧЕТ ТОЧКИ t_1\n')
    print(t_1 - (t_1 % 0.1))
    while t < t_1:

        if round(t, 1) == round(t_1 - (t_1 % 0.1),3):
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))
            # первый шаг с тягой до критической точки t_1
            Vx, Vy, x, y, m = RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, t_1 % 0.1)
            t += t_1 % 0.1
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))

            # второй шаг без тяги от критической точки t_1
            Vx, Vy, x, y, m = RK4(t, 0, m, x, y, Vx, Vy, theta_1_dot, theta_2, delta_t - t_1 % 0.1)
            t += delta_t - t_1 % 0.1
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))

        else:
            Vx, Vy, x, y, m = RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt)
            t += dt
            #print(t,round(t, 1))

    print('\nРАСЧЕТ ТОЧКИ t_2\n')

    while t < t_2:

        if round(t, 1) == t_2 - (t_2 % 0.1):
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))
            # первый шаг без тяги до критической точки t_2
            Vx, Vy, x, y, m = RK4(t, 0, m, x, y, Vx, Vy, theta_1_dot, theta_2, t_2 % 0.1)
            t += t_2 % 0.1
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))

            # второй шаг с тягой от критической точки t_2
            Vx, Vy, x, y, m = RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, delta_t - t_2 % 0.1)
            t += delta_t - t_2 % 0.1
            print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', round(t, 3))

        else:
            Vx, Vy, x, y, m = RK4(t, 0, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt)
            t += dt

    print('\nРАСЧЕТ 2АУТ И КОНЕЧНОЙ ТОЧКИ\n')

    while abs(np.sqrt(Vx ** 2 + Vy ** 2) - (np.sqrt(mu_luna / (R_luna + h)))) > (10 ** (-7)):
        Vx_do = Vx
        Vy_do = Vy
        x_do = x
        y_do = y
        m_do = m
        Vx, Vy, x, y, m = RK4(t, P, m, x, y, Vx, Vy, theta_1_dot, theta_2, dt)
        if np.sqrt(Vx ** 2 + Vy ** 2) > (np.sqrt(mu_luna / (R_luna + h))):
            Vx = Vx_do
            Vy = Vy_do
            x = x_do
            y = y_do
            m = m_do
            dt /= 10
        else:
            t += dt
        if round(m, ) < m_0 - delta_m_t_max:
            Vx = Vx_do
            Vy = Vy_do
            x = x_do
            y = y_do
            m = m_do
            t -= dt
            break

    print(Vx, ',', Vy, ',', x, ',', y, ',', m, ',', t)
    print('Полученная скорость:', np.sqrt(Vx ** 2 + Vy ** 2))
    print('Действительная скорость:', np.sqrt(mu_luna / (R_luna + h)))

    return Vx, Vy, x, y, m, t


Vx, Vy, x, y, m, t = integration(t, P, m_0, x, y, Vx, Vy, theta_1_pr, theta_2, delta_t, t_1, t_2)