From 87c469c98d73ac8ce8d166f421383b02d477990d Mon Sep 17 00:00:00 2001 From: Arne van Iterson Date: Wed, 29 Nov 2023 12:53:02 +0100 Subject: [PATCH] Python files --- src/ev3.py | 0 src/sim/line.py | 0 src/sim/pendulum.py | 88 +++++++++++++++++++++++++++++++++++++++++++++ src/sim/pid.py | 2 ++ src/sim/sim.py | 74 ++++++++++++++++++++++++++++++++++++++ 5 files changed, 164 insertions(+) create mode 100644 src/ev3.py create mode 100644 src/sim/line.py create mode 100644 src/sim/pendulum.py create mode 100644 src/sim/pid.py create mode 100644 src/sim/sim.py diff --git a/src/ev3.py b/src/ev3.py new file mode 100644 index 0000000..e69de29 diff --git a/src/sim/line.py b/src/sim/line.py new file mode 100644 index 0000000..e69de29 diff --git a/src/sim/pendulum.py b/src/sim/pendulum.py new file mode 100644 index 0000000..37787bc --- /dev/null +++ b/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 diff --git a/src/sim/pid.py b/src/sim/pid.py new file mode 100644 index 0000000..65c224d --- /dev/null +++ b/src/sim/pid.py @@ -0,0 +1,2 @@ +class PID: + def __init__: \ No newline at end of file diff --git a/src/sim/sim.py b/src/sim/sim.py new file mode 100644 index 0000000..503c63d --- /dev/null +++ b/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()