89 lines
3.1 KiB
Python
89 lines
3.1 KiB
Python
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
|