Final changes formatting and spelling
This commit is contained in:
parent
fa0427b564
commit
0295758be6
225
doc/pendulum.tex
225
doc/pendulum.tex
@ -1,6 +1,4 @@
|
||||
% This is a simple sample document. For more complicated documents take a look in the exercise tab. Note that everything that comes after a % symbol is treated as comment and ignored when the code is compiled.
|
||||
|
||||
\documentclass{article} % \documentclass{} is the first command in any LaTeX code. It is used to define what kind of document you are creating such as an article or a book, and begins the document preamble
|
||||
\documentclass{article}
|
||||
|
||||
\usepackage[english]{babel}
|
||||
\usepackage[a4paper,top=2cm,bottom=2cm,left=2cm,right=2cm,marginparwidth=1.75cm]{geometry}
|
||||
@ -18,13 +16,11 @@
|
||||
\large{EV5 - Modelling and Control}
|
||||
}
|
||||
|
||||
\author{Arne van Iterson} % Sets authors name
|
||||
\date{\today} % Sets date for date compiled
|
||||
|
||||
% The preamble ends with the command \begin{document}
|
||||
\begin{document} % All begin commands must be paired with an end command somewhere
|
||||
\maketitle % creates title using information in preamble (title, author, date)
|
||||
\author{Arne van Iterson}
|
||||
\date{\today}
|
||||
|
||||
\begin{document}
|
||||
\maketitle
|
||||
|
||||
\begin{abstract}
|
||||
Electrical engineering students at the University of applied sciences Utrecht are assigned to simulate, build and control an inherently unstable system during the 'Modelling and Control' course of the third year. This paper describes the process of doing just that for a single inverted pendulum. The pendulum itself is built using LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3. The control system is a PID-loop is implemented using Python. A digital twin, also built in Python, is used to simulate the system.
|
||||
@ -32,7 +28,7 @@
|
||||
|
||||
\noindent\makebox[\linewidth]{\rule{\linewidth}{0.4pt}}
|
||||
|
||||
\begin{multicols}{2} % creates two columns
|
||||
\begin{multicols}{2}
|
||||
% Theory
|
||||
\section{Theory}
|
||||
\label{section:theory}
|
||||
@ -188,7 +184,7 @@
|
||||
|
||||
% Make clear that the simulation is built from the ground up and pygame is only used for the UI
|
||||
\subsection{Simulation}
|
||||
The formula's in the previous sections have then been put into a purpose built Python simulation suite called Pendulum Simulator 4000. The source code for this can be found in the appendix and on git.
|
||||
The formula's in the previous sections have then been put into a purpose built Python simulation suite called Pendulum Simulator 4000. The source code for this can be found in the appendix and in a Git repository, which can be found at \url{https://arnweb.nl/gitea/arne/EV5_Modcon}.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/screenshot2.png}
|
||||
\caption{Main window of Pendulum Simulator 4000}
|
||||
@ -199,7 +195,7 @@
|
||||
\section{Setup}
|
||||
To simplify hardware design, the system will be built and run on a LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3 development kit. The programmable brick itself, hereinafter referred to as "EV3", runs a Linux distribution known as ev3dev. Ev3dev allows for various programming languages to be used including Python and C. Both of which will be further explained in section \ref{section:control}.
|
||||
|
||||
To reproduce the robot in Figure \ref{fig:balancer}, please refer to the building instructions. \cite{instruction}.
|
||||
To reproduce the robot in figure \ref{fig:balancer}, please refer to the building instructions. \cite{instruction}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png}
|
||||
@ -220,7 +216,6 @@
|
||||
In order to get the system to balance at all, the control loop had to be altered to include the position and speed of the motors as has been done in the HTWay project by D. Lechner \cite{htway}. The theory behind this is that if the system is still falling in a certain direction while the motors are already moving to compensate that, the movement speed should be increased. In a way the system now uses a cascaded control loop to balance.
|
||||
|
||||
A small set of data points has been collected by pre-assigning memory to a numpy array and writing to it directly, then dumping this to non-volatile memory. This gives some insight into the issues with the system, however, this also limits the run time of the system.
|
||||
|
||||
\section{Validation}
|
||||
The behaviour of the system is as follows:
|
||||
|
||||
@ -247,7 +242,7 @@
|
||||
Drawing any comparisons between the two will be largely useless at this point, so instead we will limit the simulation to check if the simulation will then mimic the actual behaviour.
|
||||
|
||||
\subsection{Limiting the simulated system}
|
||||
First, the resolution of the gyroscope will be limited to 1 degree. Converting this to radians gives a resolution of 0.0175 rad. The result can be seen in \ref{fig:simreslim1}. Note that the graph has been cropped, showing only a stable part of the simulation.
|
||||
First, the resolution of the gyroscope will be limited to 1 degree. Converting this to radians gives a resolution of 0.0175 rad. The result can be seen in figure \ref{fig:simreslim1}. Note that the graph has been cropped, showing only a stable part of the simulation.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_resolutionlimit1.png}
|
||||
\caption{Simulation run with angle resolution limit}
|
||||
@ -258,7 +253,7 @@
|
||||
\\\\
|
||||
Next, the simulation will be limited in update speed. This means that the physics simulation will continue while the control loop can only update every certain interval. The log file from the physical model reveals that the average update time for the control loop is about 21 ms or 47 frames per second (fps).
|
||||
|
||||
Currently, the simulation runs at 60 fps, the update call for the pendulum control is called every frame. The pendulum control has been changed to only actually control the system after the dt value is more or equal to 21 ms. Because the program runs at 60 fps or roughly 16 ms per frame, the actual PID loop will be called every 32 ms; This is longer than required, but should demonstrate the effect of the delay. The result can be seen in \ref{fig:simpidlim1}.
|
||||
Currently, the simulation runs at 60 fps, the update call for the pendulum control is called every frame. The pendulum control has been changed to only actually control the system after the dt value is more or equal to 21 ms. Because the program runs at 60 fps or roughly 16 ms per frame, the actual PID loop will be called every 32 ms; This is longer than required, but should demonstrate the effect of the delay. The result can be seen in figure \ref{fig:simpidlim1}.
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_pidcontroldelay.png}
|
||||
\caption{Simulation run with PID control delay}
|
||||
@ -277,7 +272,7 @@
|
||||
\end{split}
|
||||
\end{equation}
|
||||
|
||||
The simulation has been changed to never exceed the speed limit of 0.403 m/s even if the controller asks for it, just like the EV3 motors would do. Limiting the simulation to 0.403 m/s did little to change the behaviour of the simulation. As can be seen in \ref{fig:simslms}, the system never reaches this speed in the first place.
|
||||
The simulation has been changed to never exceed the speed limit of 0.403 m/s even if the controller asks for it, just like the EV3 motors would do. Limiting the simulation to 0.403 m/s did little to change its behaviour. As can be seen in figure \ref{fig:simslms}, the system never reaches this speed in the first place.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit.png}
|
||||
@ -285,7 +280,7 @@
|
||||
\label{fig:simslms}
|
||||
\end{figure}
|
||||
|
||||
Interesting however is the acceleration as can be seen in \ref{fig:simslms2}.
|
||||
Interesting, however, is the acceleration as can be seen in figure \ref{fig:simslms2}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit_ms2.png}
|
||||
@ -312,7 +307,7 @@
|
||||
\label{fig:simalllim1}
|
||||
\end{figure}
|
||||
|
||||
Lastly, the sensor will be offset with about 0,5 degrees per second which will be updated every time the control loop is run. The results can be seen in \ref{fig:simdrift1}.
|
||||
Lastly, the sensor will be offset with about 0,5 degrees per second which will be updated every time the control loop is run. The results can be seen in figure \ref{fig:simdrift1}.
|
||||
|
||||
\begin{figure}[H]
|
||||
\includegraphics[width=\linewidth]{./img/sim_run_alllimits2.png}
|
||||
@ -388,7 +383,7 @@
|
||||
|
||||
# Pendulum setup
|
||||
# Start angle in radians, length, mass, color
|
||||
pendulum = Pendulum(0, 2, 0, 0.25, "red")
|
||||
pendulum = Pendulum(0, 0.2, 0, 0.7, "red")
|
||||
pendulum.reset()
|
||||
dx = 0 # x offset
|
||||
dt = 1 # delta time
|
||||
@ -463,9 +458,9 @@
|
||||
ui.grid(50, 0, 15)
|
||||
|
||||
# Update PID values
|
||||
pendulum.kp = slider_kp.getValue()
|
||||
pendulum.ki = slider_ki.getValue()
|
||||
pendulum.kd = slider_kd.getValue()
|
||||
# pendulum.kp = slider_kp.getValue()
|
||||
# pendulum.ki = slider_ki.getValue()
|
||||
# pendulum.kd = slider_kd.getValue()
|
||||
|
||||
# Update pendulum
|
||||
if not pendulum.fallen:
|
||||
@ -493,8 +488,7 @@
|
||||
pygame.display.flip()
|
||||
dt = clock.tick(60) # limits FPS to 120
|
||||
|
||||
pygame.quit()
|
||||
|
||||
pygame.quit()
|
||||
\end{lstlisting}
|
||||
\subsection{pendulum.py}
|
||||
\begin{lstlisting}[language=Python]
|
||||
@ -510,6 +504,9 @@
|
||||
C_P_ANG_START = 1 / 1000 * math.pi
|
||||
C_FALL_ANG = 52.5 / 100 * math.pi
|
||||
|
||||
C_ANG_RES_LIMIT = ((2 * math.pi) / 360) # rad
|
||||
C_V_CART_LIMIT = 0.403 # m/s
|
||||
C_A_CART_LIMIT = C_V_CART_LIMIT / 0.1 # m/s^2
|
||||
|
||||
class Pendulum:
|
||||
def __init__(self, theta, length, dx, mass, color):
|
||||
@ -535,6 +532,9 @@
|
||||
self.theta = [theta] # Angle in radians
|
||||
self.a_ang = [0] # Angular acceleration
|
||||
self.v_ang = [0] # Angular velocity
|
||||
|
||||
## Gyro offset
|
||||
self.theta_offset = 0
|
||||
|
||||
self.dx = dx # Horizontal displacement of "cart" from center
|
||||
self.a_cart = [0] # Acceleration of cart
|
||||
@ -549,9 +549,16 @@
|
||||
|
||||
## PID variables
|
||||
self.pid = True
|
||||
self.kp = 1.3
|
||||
self.pidDelay = 0
|
||||
# self.kp = 1.3
|
||||
# self.ki = 0.0
|
||||
# self.kd = 0.1
|
||||
self.kp = 7.5
|
||||
self.ki = 0.0
|
||||
self.kd = 0.1
|
||||
self.kd = 1.15
|
||||
|
||||
self.kp_m = 0.07
|
||||
self.kd_m = 0.1
|
||||
|
||||
def update(self, dt):
|
||||
self.doMath(dt)
|
||||
@ -560,7 +567,12 @@
|
||||
)
|
||||
|
||||
if self.pid:
|
||||
self.pidControl(dt)
|
||||
self.pidDelay += dt
|
||||
|
||||
if self.pidDelay > 21:
|
||||
self.pidControl(self.pidDelay)
|
||||
self.theta_offset += self.pidDelay * (C_ANG_RES_LIMIT / 1000) ## One degree a second
|
||||
self.pidDelay = 0
|
||||
|
||||
if abs(self.theta[self.index]) == C_FALL_ANG:
|
||||
self.fallen = True
|
||||
@ -589,14 +601,14 @@
|
||||
|
||||
def doMath(self, dt):
|
||||
### ANGLE ###
|
||||
ang_term1 = self.a_cart[self.index] * math.cos(self.theta[self.index])
|
||||
ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index])
|
||||
ang_term1 = self.a_cart[self.index] * math.cos(self.theta[self.index] + self.theta_offset)
|
||||
ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index] + self.theta_offset)
|
||||
ang_term3 = (
|
||||
self.v_cart[self.index] # Previous cart velocity
|
||||
* self.v_ang[self.index] # previous angle velocity
|
||||
* math.sin(self.theta[self.index]) # Sin previous angle
|
||||
* math.sin(self.theta[self.index] + self.theta_offset) # Sin previous angle
|
||||
)
|
||||
ang_term4 = C_GRAVITY * math.sin(self.theta[self.index])
|
||||
ang_term4 = C_GRAVITY * math.sin(self.theta[self.index] + self.theta_offset)
|
||||
|
||||
# Angular acceleration
|
||||
self.a_ang.append(
|
||||
@ -610,10 +622,13 @@
|
||||
)
|
||||
|
||||
# Angular displacement
|
||||
self.theta.append(
|
||||
self.theta[self.index] # Previous angle
|
||||
+ (self.v_ang[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
theta_res = self.theta[self.index] + (self.v_ang[self.index + 1] * (dt / 1000))
|
||||
theta_round = theta_res % C_ANG_RES_LIMIT
|
||||
if theta_round > 0.5 * C_ANG_RES_LIMIT:
|
||||
theta_res = theta_res + theta_round
|
||||
else:
|
||||
theta_res = theta_res - theta_round
|
||||
self.theta.append(theta_res)
|
||||
|
||||
# Limit fall of pendulum
|
||||
self.theta[self.index + 1] = self.clamp(
|
||||
@ -635,13 +650,20 @@
|
||||
)
|
||||
|
||||
# Cart acceleration
|
||||
self.a_cart.append((-cart_term1 + cart_term2) / (2 * self.mass))
|
||||
a_res = (-cart_term1 + cart_term2) / (2 * self.mass)
|
||||
self.a_cart.append(self.clamp(a_res, -C_A_CART_LIMIT, C_A_CART_LIMIT))
|
||||
# if abs(a_res) < C_A_CART_LIMIT:
|
||||
# self.a_cart.append(a_res)
|
||||
# else:
|
||||
# self.a_cart.append(C_A_CART_LIMIT * (a_res / abs(a_res)))
|
||||
|
||||
# Integrate acceleration to get velocity
|
||||
self.v_cart.append(
|
||||
self.v_cart[self.index] # Previous velocity
|
||||
+ (self.a_cart[self.index + 1] * (dt / 1000))
|
||||
)
|
||||
v_res = self.v_cart[self.index] + (self.a_cart[self.index + 1] * (dt / 1000)) # Previous velocity
|
||||
|
||||
if abs(v_res) < C_V_CART_LIMIT:
|
||||
self.v_cart.append(v_res)
|
||||
else:
|
||||
self.v_cart.append(C_V_CART_LIMIT * (v_res / abs(v_res)))
|
||||
|
||||
# Cart displacement
|
||||
self.s_cart.append(
|
||||
@ -667,6 +689,7 @@
|
||||
self.a_cart = [0]
|
||||
self.v_cart = [0]
|
||||
self.s_cart = [0]
|
||||
self.theta_offset = 0
|
||||
self.fallen = False
|
||||
self.update(0)
|
||||
|
||||
@ -694,11 +717,15 @@
|
||||
plt.show()
|
||||
|
||||
def pidControl(self, dt):
|
||||
error = self.theta[self.index]
|
||||
# (dt/1000)
|
||||
error = self.theta[self.index]
|
||||
dt = (dt/1000)
|
||||
# result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index]) #PID
|
||||
result = (self.kp * error) + (((error - self.theta[self.index - 1]) / (dt + 1 / 1000)) * self.kd) #PD
|
||||
self.a_cart[self.index] = result * 10
|
||||
result = (self.kp * error) \
|
||||
+ (((error - self.theta[self.index - 1]) / (dt + 1 / 1000)) \
|
||||
* self.kd) \
|
||||
+ (self.kp_m * self.s_cart[self.index]) \
|
||||
+ (self.kd_m * self.v_cart[self.index])
|
||||
self.a_cart[self.index] = self.clamp(result * 10, -C_A_CART_LIMIT, C_A_CART_LIMIT)
|
||||
\end{lstlisting}
|
||||
\subsection{uiHelpers.py}
|
||||
\begin{lstlisting}[language=Python]
|
||||
@ -821,7 +848,6 @@
|
||||
self.blink = not self.blink
|
||||
self.blinkTimer = 0
|
||||
self.blinkTimer += dt
|
||||
|
||||
\end{lstlisting}
|
||||
\newpage
|
||||
\section{Control source code}
|
||||
@ -837,18 +863,25 @@
|
||||
from ev3dev2.sound import Sound
|
||||
|
||||
import time
|
||||
import logging
|
||||
# import logging
|
||||
import os
|
||||
import csv
|
||||
|
||||
from datetime import datetime
|
||||
|
||||
now = datetime.now() # current date and time
|
||||
LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + ".csv"))
|
||||
# print(LOGNAME)
|
||||
logging.basicConfig(filename=LOGNAME,
|
||||
filemode='a',
|
||||
format='%(message)s',
|
||||
level=logging.DEBUG)
|
||||
# LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + "final.csv"))
|
||||
# # print(LOGNAME)
|
||||
# logging.basicConfig(filename=LOGNAME,
|
||||
# filemode='a',
|
||||
# format='%(message)s',
|
||||
# level=logging.DEBUG)
|
||||
|
||||
log_t = []
|
||||
log_ang = []
|
||||
log_vang = []
|
||||
log_x = []
|
||||
log_v = []
|
||||
|
||||
# Motors
|
||||
l_motor = LargeMotor(OUTPUT_B)
|
||||
@ -859,54 +892,90 @@
|
||||
|
||||
# Sound
|
||||
sound = Sound()
|
||||
sound.speak('3, 2, 1')
|
||||
|
||||
def reset_gyro():
|
||||
gyro.calibrate()
|
||||
gyro.mode = GyroSensor.MODE_GYRO_G_A
|
||||
# if gyro.mode == gyro.MODE_GYRO_CAL:
|
||||
# gyro.mode = gyro.MODE_GYRO_G_A
|
||||
# gyro.mode = gyro.MODE_GYRO_CAL
|
||||
# else:
|
||||
# mode = gyro.mode
|
||||
# gyro.mode = gyro.MODE_GYRO_CAL
|
||||
# gyro.mode = mode
|
||||
|
||||
# tires are 56mm diameter
|
||||
def balance(target_angle=0, k_p=150, k_i=0, k_d=0.1):
|
||||
integrated_error = 0
|
||||
def balance(target_angle=0):
|
||||
t = time.time()
|
||||
|
||||
# PID
|
||||
gyro_k_p = 7.5
|
||||
# float k_i = 0 # No integral in this system
|
||||
gyro_k_d = 1.15
|
||||
motor_k_p = 0.07
|
||||
motor_k_d = 0.1
|
||||
|
||||
# Calibrate gyro in current position
|
||||
reset_gyro()
|
||||
gyro.calibrate()
|
||||
gyro.mode = GyroSensor.MODE_GYRO_G_A
|
||||
angle = gyro.angle
|
||||
|
||||
# Calibrate motor
|
||||
prev_sum_motor_pos = 0
|
||||
l_motor.reset()
|
||||
r_motor.reset()
|
||||
pos = 0
|
||||
|
||||
sound.speak('3, 2, 1')
|
||||
|
||||
# Stop if the robot has fallen over
|
||||
while abs(angle) < 40:
|
||||
# Keep time
|
||||
dt = time.time() - t
|
||||
t = time.time()
|
||||
|
||||
angle, rate = gyro.angle_and_rate
|
||||
error = angle - target_angle
|
||||
integrated_error = integrated_error + error * dt
|
||||
sum_motor_pos = l_motor.position + r_motor.position
|
||||
|
||||
pid = k_p * error + k_i * integrated_error + k_d * rate
|
||||
delta_motor_pos = sum_motor_pos - prev_sum_motor_pos
|
||||
pos += delta_motor_pos
|
||||
speed = (delta_motor_pos / dt)
|
||||
|
||||
prev_sum_motor_pos = sum_motor_pos
|
||||
|
||||
# print(t, angle, integrated_error, rate, pid)
|
||||
logging.debug("%d;%d;%d", angle, rate, pid)
|
||||
angle, rate = gyro.angle_and_rate
|
||||
|
||||
if abs(pid) > 1050:
|
||||
print("cap")
|
||||
log_t.append(t)
|
||||
log_ang.append(angle)
|
||||
log_vang.append(rate)
|
||||
log_x.append(pos)
|
||||
log_v.append(speed)
|
||||
|
||||
speed = min(max(pid, -1050), 1050)
|
||||
error = angle - target_angle
|
||||
pd = gyro_k_p * error + gyro_k_d * rate \
|
||||
+ motor_k_p * pos + motor_k_d * speed
|
||||
|
||||
# convert -100 ~ 100 to -1050 ~ 1050
|
||||
speed = (((pd - (-100)) * (1049 - (-1049))) / (100 - (-100))) + (-1049)
|
||||
l_motor.run_forever(speed_sp=speed)
|
||||
r_motor.run_forever(speed_sp=speed)
|
||||
|
||||
# logging.debug("%d;%d;%d", angle, rate, pd)
|
||||
|
||||
# if abs(pd) > 1050:
|
||||
# print("cap")
|
||||
|
||||
# speed = min(max(pd, -1050), 1050)
|
||||
|
||||
l_motor.stop(stop_action="hold")
|
||||
r_motor.stop(stop_action="hold")
|
||||
|
||||
balance()
|
||||
|
||||
# Combine the arrays
|
||||
try:
|
||||
balance()
|
||||
except:
|
||||
l_motor.stop(stop_action="hold")
|
||||
r_motor.stop(stop_action="hold")
|
||||
finally:
|
||||
data = zip(log_t, log_ang, log_vang, log_x, log_v)
|
||||
|
||||
# Define the filename
|
||||
now = datetime.now() # current date and time
|
||||
LOGNAME = os.path.join(now.strftime("%m-%d_%H:%M:%S") + "new.csv")
|
||||
|
||||
# Write data to CSV file
|
||||
with open(LOGNAME, 'w', newline='') as csvfile:
|
||||
csvwriter = csv.writer(csvfile)
|
||||
csvwriter.writerow(['Time', 'Angle', 'Angular Velocity', 'X pos', 'Speed']) # Write header row
|
||||
csvwriter.writerows(data)
|
||||
\end{lstlisting}
|
||||
|
||||
\end{document} % This is the end of the document
|
||||
|
||||
Loading…
Reference in New Issue
Block a user