Hello! Wrote a code that builds a model of a reverse pendulum:

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()

You need to build 2 animated graphics: phi from t and velocity from t

How to build a graph in real time, which will be built depending on the received phi and velocity while balancing the inverse pendulum?

HELP, PLEASE ANYONE ANYONE !!!

    0