-
Notifications
You must be signed in to change notification settings - Fork 2
/
pendulum2.py
96 lines (71 loc) · 2.08 KB
/
pendulum2.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
import pygame
import math
import parameters as parameter
import profiles as pf
class Bob:
def __init__(self, x, y, r):
self.x = x
self.y = y
self.r = r
self.colour = (0, 0, 255)
self.thickness = 1
def display(self, screen):
# print(self.x, " ", self.y)
pygame.draw.circle(screen, self.colour, (self.x, self.y), self.r)
class Line:
def __init__(self, basex, basey, tox, toy):
self.basex = basex
self.basey = basey
self.x = tox
self.colour = (0, 255, 0)
self.y = toy
self.length = math.sqrt((tox-basex)**2 + (toy-basey)**2)
def display(self, screen):
pygame.draw.line(screen, self.colour, [
self.basex, self.basey], [self.x, self.y], 3)
background_colour = (255, 255, 255)
(width, height) = (800, 800)
screen = pygame.display.set_mode((width, height))
pygame.display.set_caption('Inverted Pendulum')
screen.fill(background_colour)
x = 300
y = 150
# params
params = parameter.Setup()
# start angle
thita = params.start_angle
# velocity
omega = params.start_velocity
# accel
alpha = params.start_alpha
particle = Bob(x, y, 30)
basex = 300
basey = 400
line = Line(basex, basey, x, y)
pygame.display.flip()
running = True
t = params.dt
def g_factor(thita):
return params.g_value * (math.sin(math.radians(thita)))
while running:
screen.fill((255, 255, 255))
thita = thita+omega*t
omega = omega + alpha*t + g_factor(thita)*t
alpha = pf.defuzzy(params.current, params.angle,
params.velocity, thita, omega)/(params.I* params.k)
# print(thita, omega, alpha)
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
x = line.length * math.sin(math.radians(thita))+basex
y = basey - line.length * math.cos(math.radians(thita))
# print(x, " ", y)
line.x = round(x)
line.y = round(y)
particle.x = line.x
particle.y = line.y
line.display(screen)
particle.display(screen)
pygame.display.flip()
pygame.time.delay(30)
pygame.quit()