arne/EV5_Modcon
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 2 Wiki Activity

PID Control

Browse Source
This commit is contained in:
Arne van Iterson 2023-12-13 11:45:53 +01:00
parent 5e0ee34ed7
commit 5715322f78
3 changed files with 66 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

66
src/sim/pendulum.py
View File

@ -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

28
src/sim/sim.py
View File

@ -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

3
src/sim/uiHelpers.py
View File

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