diff --git a/src/sim/pendulum.py b/src/sim/pendulum.py index 3bb368b..5eaee43 100644 --- a/src/sim/pendulum.py +++ b/src/sim/pendulum.py @@ -26,9 +26,12 @@ class Pendulum: Returns: None """ + ## Game variables self.vector = None # Vector2 object + self.fallen = False # Stop when pendulum falls over - self.index = 0 + ## Physics variables + self.index = 0 # Index helper for plotting graphs self.theta = [theta] # Angle in radians self.a_ang = [0] # Angular acceleration self.v_ang = [0] # Angular velocity @@ -44,18 +47,46 @@ class Pendulum: self.mass = mass # Mass of pendulum for physics self.color = color # Display color - self.pid = False - self.fallen = False + ## PID variables + self.pid = True + self.kp = 1.3 + self.ki = 0.0 + self.kd = 0.1 def update(self, dt): self.doMath(dt) self.vector = Vector2.from_polar( ((self.length * C_MTPRATIO), math.degrees(self.theta[self.index] - (0.5 * math.pi))) ) + + if self.pid: + self.pidControl() if abs(self.theta[self.index]) == C_FALL_ANG: self.fallen = True + # def update(self, dt): + # """ + # Update the pendulum's state based on the elapsed time. + + # Parameters: + # - dt (float): The elapsed time in milliseconds. + + # Returns: + # None + # """ + # a_ang = (-(C_GRAVITY * math.sin(self.theta)) / (self.length)) - (self.r_factor * self.v_ang) # Angular acceleration + + # v_ang = a_ang * (dt/1000) + self.v_ang # Integrate acceleration to get velocity + # s_ang = v_ang * (dt/1000) # Angular displacement + + # self.theta += s_ang # Update value + + # self.vector = Vector2.from_polar(((self.length * 150), math.degrees(self.theta + math.pi/2))) + + # self.a_ang = a_ang # Update value + # self.v_ang = v_ang # Update value + def doMath(self, dt): ### ANGLE ### ang_term1 = self.a_cart[self.index] * math.cos(self.theta[self.index]) @@ -155,31 +186,14 @@ class Pendulum: axs[0,0].plot(self.s_cart) axs[0,0].set_title('Position [m]') - axs[0,1].plot(self.v_ang) + axs[0,1].plot(self.v_cart) axs[0,1].set_title('Speed [m/s]') - axs[1,0].plot(self.a_ang) + axs[1,0].plot(self.a_cart) axs[1,0].set_title('Acceleration [m/s^2]') plt.show() - # def update(self, dt): - # """ - # Update the pendulum's state based on the elapsed time. - - # Parameters: - # - dt (float): The elapsed time in milliseconds. - - # Returns: - # None - # """ - # a_ang = (-(C_GRAVITY * math.sin(self.theta)) / (self.length)) - (self.r_factor * self.v_ang) # Angular acceleration - - # v_ang = a_ang * (dt/1000) + self.v_ang # Integrate acceleration to get velocity - # s_ang = v_ang * (dt/1000) # Angular displacement - - # self.theta += s_ang # Update value - - # self.vector = Vector2.from_polar(((self.length * 150), math.degrees(self.theta + math.pi/2))) - - # self.a_ang = a_ang # Update value - # self.v_ang = v_ang # Update value + def pidControl(self): + error = self.theta[self.index] + result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index]) + self.a_cart[self.index] = result * 10 \ No newline at end of file diff --git a/src/sim/sim.py b/src/sim/sim.py index 69ee605..7db4a81 100644 --- a/src/sim/sim.py +++ b/src/sim/sim.py @@ -2,7 +2,9 @@ # Arne van Iterson, 2023 # Imports +import pygame_widgets import pygame +from pygame_widgets.slider import Slider from pygame.math import Vector2 import math @@ -28,6 +30,11 @@ pendulum.reset() dx = 0 # x offset dt = 1 # delta time +# Sliders +slider_kp = Slider(screen, 910, 590, 320, 16, initial=pendulum.kp, min=0, max=2, step=0.001) +slider_ki = Slider(screen, 910, 620, 320, 16, initial=pendulum.ki, min=0, max=0.5, step=0.001) +slider_kd = Slider(screen, 910, 650, 320, 16, initial=pendulum.kd, min=0, max=0.5, step=0.001) + # Gametime rt = 10 # run time highscore = 0 @@ -39,15 +46,22 @@ def meta(): ui.meta(pendulum.a_ang[pendulum.index], "Angular acceleration") ui.meta(pendulum.dx, "dx") ui.meta(pendulum.a_cart[pendulum.index], "Cart acceleration") + ui.meta(pendulum.pid, "Control") + + ui.meta(pendulum.kd, "Kd") + ui.meta(pendulum.ki, "Ki") + ui.meta(pendulum.kp, "Kp") + ui.meta(not update, "Paused") ui.meta(rt / 1000, "Run time [s]") ui.meta(highscore / 1000, "Highscore [s]") while running: + events = pygame.event.get() ### User controls ### - for event in pygame.event.get(): + for event in events: # Quit if event.type == pygame.QUIT: running = False @@ -57,7 +71,6 @@ while running: running = False # Reset simulation elif event.key == pygame.K_SPACE: - pendulum.reset() rt = 0 # Pause simulation @@ -67,11 +80,14 @@ while running: elif event.key == pygame.K_g: if pendulum.fallen: pendulum.plot() + else: + update = False + pendulum.plot() # Toggle PID controller elif event.key == pygame.K_c: pendulum.pid = not pendulum.pid - # Move cart + # Move pendulum keys = pygame.key.get_pressed() if keys[pygame.K_LEFT] or keys[pygame.K_a]: pendulum.a_cart[pendulum.index] -= 4 @@ -81,6 +97,11 @@ while running: # Draw grid ui.grid(50, 0, 15) + # Update PID values + pendulum.kp = slider_kp.getValue() + pendulum.ki = slider_ki.getValue() + pendulum.kd = slider_kd.getValue() + # Update pendulum if not pendulum.fallen: if update: @@ -103,6 +124,7 @@ while running: pygame.draw.circle(screen, "black", pole + dx, 15, 3) # Draw frame + pygame_widgets.update(events) pygame.display.flip() dt = clock.tick(60) # limits FPS to 120 diff --git a/src/sim/uiHelpers.py b/src/sim/uiHelpers.py index c5fab9c..57681fc 100644 --- a/src/sim/uiHelpers.py +++ b/src/sim/uiHelpers.py @@ -11,7 +11,6 @@ gridDark = pygame.Color(C_GRID_D_VALUE, C_GRID_D_VALUE, C_GRID_D_VALUE) font_h = pygame.font.SysFont(None, 28) font_m = pygame.font.SysFont(None, 16) - # UI Class class SimUI: def __init__(self, screen, pole): @@ -23,7 +22,7 @@ class SimUI: self.blink = False self.blinkTimer = 0 - self.controlled = False + self.controlled = True self.paused = False def meta(self, val, desc):