PID Control

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,8 +47,11 @@ class Pendulum:
         self.mass = mass  # Mass of pendulum for physics
         self.color = color  # Display color
 
-        self.pid = False
+        ## PID variables
-        self.fallen = False
+        self.pid = True
+        self.kp = 1.3
+        self.ki = 0.0
+        self.kd = 0.1
 
     def update(self, dt):
         self.doMath(dt)
@@ -53,9 +59,34 @@ class Pendulum:
             ((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):
+    def pidControl(self):
-    #     """
+        error = self.theta[self.index]
-    #     Update the pendulum's state based on the elapsed time.
+        result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index])
-
+        self.a_cart[self.index] = result * 10
-    #     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

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
 

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