Python files

src/sim/pendulum.py

@@ -0,0 +1,88 @@
+from pygame.math import Vector2
+import math
+import numpy as np
+
+# Constants
+C_GRAVITY = 9.81  # m/s^2
+
+
+class Pendulum:
+    def __init__(self, theta, length, dx, mass, color):
+        """
+        Initialize a Pendulum object.
+
+        Parameters:
+        theta (float): Angle in radians.
+        length (float): Length of the pendulum.
+        dx (float): Horizontal displacement of the "cart" from the center.
+        mass (float): Mass of the pendulum for physics calculations.
+        color (str): Display color.
+
+        Returns:
+        None
+        """
+        self.vector = None  # Vector2 object
+
+        self.theta = theta  # Angle in radians
+        self.a_ang = 0  # Angular acceleration
+        self.v_ang = 0  # Angular velocity
+
+        self.dx = dx  # Horizontal displacement of "cart" from center
+        self.a_cart = 0  # Acceleration of cart
+        self.v_cart = 0  # Velocity of cart
+
+        self.r_factor = 0.99  # Damping factor
+
+        self.length = length  # Length of pendulum
+        self.mass = mass  # Mass of pendulum for physics
+        self.color = color  # Display color
+
+        self.pid = False
+
+    def update(self, dt):
+        self.doMath(dt)
+        self.vector = Vector2.from_polar(
+            ((self.length * 150), math.degrees(self.theta + math.pi / 2))
+        )
+
+    def doMath(self, dt):
+        # Angle
+        ang_term1 = -(C_GRAVITY * math.sin(self.theta))
+        ang_term2 = self.a_cart * math.sin(self.theta)
+        ang_term3 = self.v_cart * self.v_ang * math.cos(self.theta)
+
+        # self.a_ang = ((ang_term1 - ang_term2 - ang_term3) / self.length) - (self.r_factor * self.v_ang) # Angular acceleration        
+        self.a_ang = ((ang_term1 - ang_term2 - ang_term3) / self.length) # Angular acceleration        
+        self.v_ang = self.a_ang * (dt / 1000) + self.v_ang  # Integrate acceleration to get velocity
+        self.theta = self.v_ang * (dt / 1000) + self.theta  # Angular displacement
+
+        # Cart pos
+        cart_term1 = self.length * self.a_ang * math.sin(self.theta)
+        cart_term2 = self.length * pow(self.v_ang, 2) * math.cos(self.theta)
+
+        self.a_cart = (cart_term1 - cart_term2) / 2  # Cart acceleration
+        self.v_cart = (self.a_cart * (dt / 1000) + self.v_cart)  # Integrate acceleration to get velocity
+        self.dx = self.v_cart * (dt / 1000) + self.dx  # Cart displacement
+
+
+    # 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

src/sim/pid.py

@@ -0,0 +1,2 @@
+class PID: 
+    def __init__:

src/sim/sim.py

@@ -0,0 +1,74 @@
+import pygame
+from pygame.math import Vector2
+import math
+
+from pendulum import Pendulum
+
+# pygame setup
+pygame.init()
+screen = pygame.display.set_mode((1280, 720))
+clock = pygame.time.Clock()
+running = True
+
+# Text setup
+font_h = pygame.font.SysFont(None, 28)
+font_m = pygame.font.SysFont(None, 16)
+
+# Metadata plotter
+plot_y = 40
+def plotMeta(val, desc):
+    global plot_y
+    screen.blit(font_m.render(f"{desc} = {val}", True, "black"), (10, plot_y))
+    plot_y += 15
+
+
+# Pendulum setup
+# Start angle in radians, length, mass, color
+p_t_start = 99 / 100 * math.pi
+pendulum = Pendulum(p_t_start, 1, 0, 100, "red")
+dt = 1  # delta time
+
+while running:
+    # poll for events
+    # pygame.QUIT event means the user clicked X to close your window
+    for event in pygame.event.get():
+        if event.type == pygame.QUIT:
+            running = False
+        if event.type == pygame.KEYDOWN:
+            keys = pygame.key.get_pressed()
+            if keys[pygame.K_SPACE]:
+                pendulum.theta = p_t_start
+            if keys[pygame.K_LEFT]:
+                pendulum.dx -= 1
+            if keys[pygame.K_RIGHT]:
+                pendulum.dx += 1
+
+    # fill the screen with a color to wipe away anything from last frame
+    screen.fill("gray")
+    pole = Vector2(screen.get_rect().center)  # center of screen
+
+    # Update pendulum
+    pendulum.update(dt)
+    dx = (pendulum.dx, 0)
+
+    # Draw metadata
+    screen.blit(font_h.render("Pendulum simulator 4000", True, "black"), (10, 10))
+    
+    plot_y = 40
+    plotMeta(pendulum.theta, "Theta")
+    plotMeta(pendulum.dx, "dx")
+    plotMeta(dt, "Frame time")
+    plotMeta(1000 / dt, "FPS")
+    plotMeta(pendulum.pid, "Control")
+
+    # Draw pendulum
+    pygame.draw.line(screen, pendulum.color, pole + dx, pole + pendulum.vector + dx, 3)
+    pygame.draw.circle(screen, "black", pole + dx, 15, 3)
+    
+    # Draw x axis
+    pygame.draw.line(screen, "black", (0, pole.y + 15), (1280, pole.y + 15), 1)
+
+    pygame.display.flip()
+    dt = clock.tick(120)  # limits FPS to 120
+
+pygame.quit()