/
Chronos
/
TU_laba
Обзор
Документация
Войти
/
Chronos
/
TU_laba
Код
Запросы
0
Задачи
Вики
Пакеты
0
Релизы
0
Аналитика
Безопасность
master
task_6.py
266 строк
10 KB
Chronos
Живо то, что ещё не умерло
28 мар 2025, 09:12
28 мар 2025, 09:12
f49a0f7
Код
Авторство
О чём код?
import numpy as np from scipy.integrate import odeint import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation from full_unimod_lib import * from params import * from A_B_computation import * # Параметры системы (задайте свои значения) # Точность вывода k = 7 # Количество знаков после запятой # Линеаризованная система имеет матрицы A = np.asarray([ [0 , 0 , 1 , 0], [0 , 0 , 0 , 1], [74.4982357 , -38.2565903, -56.0726559, 86.3428671], [-114.7697709,101.3459259,86.3838696, -228.7317749] ]).T B = np.asarray([ [1], [1], [0], [0] ]) # theta1_ddot_, theta2_ddot_ = solve_system() # Применим метод через форму Фробениуса-Луенбергера spec = np.asarray([-1+1j,-1-1j,-21.421,-265.086]) # Желаемый спектр: ЗДЕСЬ МЕНЯТЬ КОРНИ! Q,Ac = full_unimodal_control_FL(A,B,spec) # print(np.linalg.eigvals(Ac)) a1, a2, a3, a4 = Q[0] # Коэффициенты управления # def model(y, t): # theta1, theta2,theta1_dot, theta2_dot = y # # # Вычисление управляющего напряжения V # V = a1*theta1 + a2*theta2 + a3*theta1_dot + a4*theta2_dot # # # # Общий знаменатель # # delta_theta = theta1 - theta2 # # denominator = A11 * A22 - (A12**2) * np.cos(delta_theta)**2 # # # # # Расчет theta1_ddot # # numerator1 = A12 * B2 * theta2_dot * np.cos(delta_theta)- A12 * g * l2 * m2 * np.sin(theta2) * np.cos(delta_theta)- A22 * (B1 + Kf*Ks) * theta1_dot+ A22 * g * (L1 * m2 + l1 * m1) * np.sin(theta1)+ A22 * Kf * V # # # # theta1_ddot = numerator1 / denominator # # # # # Расчет theta2_ddot # # numerator2 = -A11 * B2 * theta2_dot+ A11 * g * l2 * m2 * np.sin(theta2)+ A12 * theta1_dot * (B1 + Kf*Ks) * np.cos(delta_theta)- A12 * g * (m2*L1 + m1*l1) * np.sin(theta1) * np.cos(delta_theta)- A12 * Kf * V * np.cos(delta_theta) # # # # theta2_ddot = numerator2 / denominator # # return [theta1_dot, theta2_dot,theta1_ddot_(V,theta1, theta2,theta1_dot,theta2_dot), theta2_ddot_(V,theta1, theta2,theta1_dot,theta2_dot)] def linear_model(y, t): return np.dot(Ac, y) # Временной интервал t = np.linspace(0, 10, 10_000) # Начальные условия (близкие к нулю) y0 = [0.001,0.0001, 0, 0] # y0 = [[0.1,0.1, 0, 0] # Решение нелинейной системы # sol_nonlinear = odeint(model, y0, t) # Решение линеаризованной системы sol_linear = odeint(linear_model, y0, t) # Построение графиков # plt.figure() # f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) # f.suptitle('Нелинейная система', fontsize=20,fontfamily = "Times New Roman") # fs = 20 # # Фазовые портреты нелинейной системы # # plt.subplot(2, 2, 1) # ax1.plot(t, sol_nonlinear[:,0], 'b') # ax1.set_ylabel("$\\theta_1$",fontsize = fs) # ax1.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax1.grid() # # # plt.subplot(2, 2, 2) # ax2.plot(t, sol_nonlinear[:,1], 'r') # ax2.set_ylabel("$\\theta_2$",fontsize = fs) # ax2.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax2.grid() # # # # plt.subplot(2, 2, 3) # ax3.plot(t, sol_nonlinear[:,2], 'b') # ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) # ax3.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax3.grid() # # # plt.subplot(2, 2, 4) # ax4.plot(t, sol_nonlinear[:,3], 'r') # ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) # ax4.set_xlabel('$t$',fontsize = fs) # # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # # # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) # ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax4.grid() f, ((ax1,ax2),(ax3,ax4)) = plt.subplots(2,2) f.suptitle('Линейная система', fontsize=20,fontfamily = "Times New Roman") fs = 20 # Фазовые портреты нелинейной системы # plt.subplot(2, 2, 1) ax1.plot(t, sol_linear[:,0], 'b') ax1.set_ylabel("$\\theta_1$",fontsize = fs) ax1.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax1.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax1.grid() # plt.subplot(2, 2, 2) ax2.plot(t, sol_linear[:,1], 'r') ax2.set_ylabel("$\\theta_2$",fontsize = fs) ax2.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax2.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax2.grid() # plt.subplot(2, 2, 3) ax3.plot(t, sol_linear[:,2], 'b') ax3.set_ylabel("$\\dot{\\theta}_1$",fontsize = fs) ax3.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_1$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax3.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax3.grid() # plt.subplot(2, 2, 4) ax4.plot(t, sol_linear[:,3], 'r') ax4.set_ylabel("$\\dot{\\theta}_2$",fontsize = fs) ax4.set_xlabel('$t$',fontsize = fs) # plt.title('Нелинейная система: фазовый портрет $\\theta_2$',fontfamily = "Times New Roman",fontsize = fs) # plt.xticks(fontfamily = "Times New Roman",fontsize = fs-2) # plt.yticks(fontfamily = "Times New Roman",fontsize = fs) ax4.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') ax4.grid() # f,ax = plt.subplots(1,1) # f.suptitle("Управление",fontsize=20,fontfamily = "Times New Roman") # ax.plot(t,a1*sol_nonlinear[:,0] + a2*sol_nonlinear[:,1] + a3*sol_nonlinear[:,2] + a4*sol_nonlinear[:,3],'b') # ax.tick_params(axis='both', which='major', labelsize=fs, labelfontfamily='Times New Roman') # ax.set_xlabel('$t$',fontsize = fs) # ax.set_ylabel('$V$',fontsize = fs) # ax.grid() # step = 10 # Шаг прореживания данных # t_anim = t[::step] # sol_anim = sol_nonlinear[::step] # # # Создаем фигуру для анимации # fig_anim, ax = plt.subplots(figsize=(6,6)) # ax.set_xlim(-0.6, 0.6) # ax.set_ylim(0, L1+L2+0.5) # ax.set_aspect('equal') # ax.grid() # # # Создаем элементы анимации # line1, = ax.plot([], [], 'b-', lw=2) # Первое звено # line2, = ax.plot([], [], 'r-', lw=2) # Второе звено # point1, = ax.plot([], [], 'bo', markersize=10) # Шарнир 1 # point2, = ax.plot([], [], 'ro', markersize=10) # Шарнир 2 # time_text = ax.text(0.05, 0.9, '', transform=ax.transAxes) # # def init(): # line1.set_data([], []) # line2.set_data([], []) # point1.set_data([], []) # point2.set_data([], []) # time_text.set_text('') # return line1, line2, point1, point2, time_text # # def animate(i): # theta1 = sol_anim[i, 0] # theta2 = sol_anim[i, 2] # # # Координаты первого звена # x1 = L1 * np.sin(theta1) # y1 = L1 * np.cos(theta1) # # # Координаты второго звена # x2 = x1 + L2 * np.sin(theta2) # y2 = y1 + L2 * np.cos(theta2) # # # Обновление линий # line1.set_data([0, x1], [0, y1]) # line2.set_data([x1, x2], [y1, y2]) # # # Обновление точек # point1.set_data([x1], [y1]) # point2.set_data([x2], [y2]) # # time_text.set_text(f'Время = {t_anim[i]:.2f} с') # return line1, line2, point1, point2, time_text # # # Создаем анимацию # ani = FuncAnimation(fig_anim, animate, frames=len(sol_anim), # init_func=init, blit=True, interval=20) # ДЛЯ СОХРАНЕНИЯ АНИМАЦИИ !!!!! # plt.rcParams['animation.ffmpeg_path'] = 'C:\\ffmpeg\\ffmpeg-master-latest-win64-gpl-shared\\bin\\ffmpeg.exe' # ani.save('task_5d.mp4', fps=30) plt.tight_layout() plt.show()