Новичок
Джуниор
Регистрация: 22.05.2017
Сообщений: 1
|
Построение графиков в python
Здравствуйте! Нужно создать 2 графика: phi от t и velocity от t, причём графики должны строиться во время движения обратного маятника, то есть быть ещё и анимированными, чтобы можно было отследить, как меняется угол отклонения обратного маятника и скорость тележки со временем.
Вот мой код с моделью обратного маятника:
Код:
from math import *
import pygame
import numpy as np
dt = 0.01
g = 9.81
l = 1.0
m = 1.0
global k1, k2, k3
k1 = 5000 #пропорциональный коэффициент
k2 = 100 #дифференциальный коэффициент
k3 = 1 #интегральный коэффициент
clock = pygame.time.Clock()
pygame.init()
size = (width, height) = (1200, 600)
screen = pygame.display.set_mode(size)
class Pendulum:
def __init__(self, x0, y0, phi0):
self.phi0 = phi0
self.phi = phi0
self.velocity = 0
self.x0 = x0
self.y0 = y0
self.x0_vel = 0
self.x = x0 + 250.0 * sin(phi0)
self.y = y0 + 250.0 * cos(phi0)
self.t = dt
self.t = np.arange(0, 30, 0.01)
def move(self, control):
self.phi = atan2(self.x - self.x0, self.y - self.y0) #arctg(y/x)
d_velocity = -g * sin(self.phi) * dt / l
self.velocity += d_velocity
d_phi = dt * self.velocity
self.phi += d_phi
self.x = self.x0 + 250.0 * sin(self.phi)
self.y = self.y0 + 250.0 * cos(self.phi)
d_x0_vel = dt * control
self.x0_vel += d_x0_vel
dx0 = dt * self.x0_vel
self.x0 += dx0
if self.x0 > 1200 or self.x0 < 0:
self.x0 -= dx0
def draw(self):
pygame.draw.circle(screen, (0, 0, 0), [int(self.x0), int(self.y0)], 5)
pygame.draw.line(screen, (0, 0, 0), [self.x0, self.y0], [self.x, self.y], 2)
pygame.draw.circle(screen, (255, 0, 0), [int(self.x), int(self.y)], 10)
pygame.draw.line(screen, (0, 0, 0), [0, self.y0], [1200, self.y0], 3)
class PID:
def __init__(self, k1, k2, k3, pendulum):
self.k1 = k1
self.k2 = k2
self.k3 = k3
self.p = pendulum
self.error = pi - self.p.phi
self.derivative = 0
self.integral = 0
def update(self):
self.k1 = k1
self.k2 = k2
self.k3 = k3
tmp = self.error
self.error = copysign(1, p.phi) * (pi - abs(self.p.phi)) + (self.p.x0 - 600) / 10000
diff = self.error - tmp
self.derivative = diff / dt
self.integral += tmp
# print self.error
def output(self):
return self.k1 * self.error + self.k2 * self.derivative + self.k3 * self.integral
def pid_tune(pid):
global k1, k2, k3
# new_pend = Pendulum(pend.x0, pend.y0, pend.phi)
# new_pid = PID(pid.k1, pid.k2, pid.k3, new_pend)
# new_pid.derivative = pid.derivative
# new_pid.integral = pid.integral
n_params = 3
init_params = [pid.k1, pid.k2, pid.k3]
params = [0.0 for i in range(n_params)]
for i in range(n_params):
params[i] = init_params[i]
def draw_text():
myfont = pygame.font.SysFont("monospace", 15)
label1 = myfont.render("Пропорциональный коэффициент: %d" % k1, 1, (255, 0, 0))
screen.blit(label1, (100, 500))
label2 = myfont.render("Дифференциальный коэффициент: %d" % k2, 1, (255, 0, 0))
screen.blit(label2, (100, 520))
label3 = myfont.render("Интегральный коэффициент: %.1f" % k3, 1, (255, 0, 0))
screen.blit(label3, (100, 540))
def make_buttons(k1, k2, k3, pid):
pygame.draw.rect(screen, (0, 0, 255), [320, 500, 90, 15]) #k1+
pygame.draw.rect(screen, (0, 0, 255), [420, 500, 90, 15]) #k1-
pygame.draw.rect(screen, (0, 0, 255), [320, 520, 90, 15]) #k2+
pygame.draw.rect(screen, (0, 0, 255), [420, 520, 90, 15]) #k2-
pygame.draw.rect(screen, (0, 0, 255), [320, 540, 90, 15]) #k3+
pygame.draw.rect(screen, (0, 0, 255), [420, 540, 90, 15]) #k3-
myfont = pygame.font.SysFont("monospace", 15)
label1 = myfont.render("Повысить", 1, (255, 255, 255))
label2 = myfont.render("Понизить", 1, (255, 255, 255))
screen.blit(label1, (320, 500))
screen.blit(label2, (420, 500))
screen.blit(label1, (320, 520))
screen.blit(label2, (420, 520))
screen.blit(label1, (320, 540))
screen.blit(label2, (420, 540))
if pygame.mouse.get_pressed()[0]:
(pos1, pos2) = pygame.mouse.get_pos()
if 320 <= pos1 <= 410 and 500 <= pos2 <= 515:
k1 += 10
elif 420 <= pos1 <= 490 and 500 <= pos2 <= 515:
k1 -= 10
elif 320 <= pos1 <= 410 and 520 <= pos2 <= 535:
k2 += 1
elif 420 <= pos1 <= 490 and 520 <= pos2 <= 535:
k2 -= 1
elif 320 <= pos1 <= 410 and 540 <= pos2 <= 555:
k3 += 0.1
elif 420 <= pos1 <= 490 and 540 <= pos2 <= 555:
k3 -= 0.1
elif 600 <= pos1 <= 720 and 500 <= pos2 <= 555:
pid_tune(pid)
return k1, k2, k3
p = Pendulum(600, 300, pi - pi / 10)
pid = PID(k1, k2, k3, p)
while 1:
screen.fill((255, 255, 255))
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
pid.update()
p.move(pid.output())
p.draw()
draw_text()
(k1, k2, k3) = make_buttons(k1, k2, k3, pid)
clock.tick(60)
pygame.display.flip()
Помогите, пожалуйста! Питон для меня новый язык. Знаю пока только основные библиотеки и недавно познакомился с pygame, но в pygame график построить не удаётся, а matplotlib недостаточно.
|