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

Final changes formatting and spelling

Browse Source
This commit is contained in:
Arne van Iterson 2024-02-11 19:46:53 +01:00
parent fa0427b564
commit 0295758be6
1 changed files with 147 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

223
doc/pendulum.tex
View File

@ -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{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
\usepackage[english]{babel} \usepackage[english]{babel}
\usepackage[a4paper,top=2cm,bottom=2cm,left=2cm,right=2cm,marginparwidth=1.75cm]{geometry} \usepackage[a4paper,top=2cm,bottom=2cm,left=2cm,right=2cm,marginparwidth=1.75cm]{geometry}
@ -18,13 +16,11 @@
\large{EV5 - Modelling and Control} \large{EV5 - Modelling and Control}
} }
\author{Arne van Iterson} % Sets authors name \author{Arne van Iterson}
\date{\today} % Sets date for date compiled \date{\today}
% 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)
\begin{document}
\maketitle
\begin{abstract} \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. 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}} \noindent\makebox[\linewidth]{\rule{\linewidth}{0.4pt}}
\begin{multicols}{2} % creates two columns \begin{multicols}{2}
% Theory % Theory
\section{Theory} \section{Theory}
\label{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 % Make clear that the simulation is built from the ground up and pygame is only used for the UI
\subsection{Simulation} \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] \begin{figure}[H]
\includegraphics[width=\linewidth]{../res/screenshot2.png} \includegraphics[width=\linewidth]{../res/screenshot2.png}
\caption{Main window of Pendulum Simulator 4000} \caption{Main window of Pendulum Simulator 4000}
@ -199,7 +195,7 @@
\section{Setup} \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 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] \begin{figure}[H]
\includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png} \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. 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. 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} \section{Validation}
The behaviour of the system is as follows: 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. 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} \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] \begin{figure}[H]
\includegraphics[width=\linewidth]{./img/sim_run_resolutionlimit1.png} \includegraphics[width=\linewidth]{./img/sim_run_resolutionlimit1.png}
\caption{Simulation run with angle resolution limit} \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). 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] \begin{figure}[H]
\includegraphics[width=\linewidth]{./img/sim_run_pidcontroldelay.png} \includegraphics[width=\linewidth]{./img/sim_run_pidcontroldelay.png}
\caption{Simulation run with PID control delay} \caption{Simulation run with PID control delay}
@ -277,7 +272,7 @@
\end{split} \end{split}
\end{equation} \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] \begin{figure}[H]
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit.png} \includegraphics[width=\linewidth]{./img/sim_run_speedlimit.png}
@ -285,7 +280,7 @@
\label{fig:simslms} \label{fig:simslms}
\end{figure} \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] \begin{figure}[H]
\includegraphics[width=\linewidth]{./img/sim_run_speedlimit_ms2.png} \includegraphics[width=\linewidth]{./img/sim_run_speedlimit_ms2.png}
@ -312,7 +307,7 @@
\label{fig:simalllim1} \label{fig:simalllim1}
\end{figure} \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] \begin{figure}[H]
\includegraphics[width=\linewidth]{./img/sim_run_alllimits2.png} \includegraphics[width=\linewidth]{./img/sim_run_alllimits2.png}
@ -388,7 +383,7 @@
# Pendulum setup # Pendulum setup
# Start angle in radians, length, mass, color # 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() pendulum.reset()
dx = 0 # x offset dx = 0 # x offset
dt = 1 # delta time dt = 1 # delta time
@ -463,9 +458,9 @@
ui.grid(50, 0, 15) ui.grid(50, 0, 15)
# Update PID values # Update PID values
pendulum.kp = slider_kp.getValue() # pendulum.kp = slider_kp.getValue()
pendulum.ki = slider_ki.getValue() # pendulum.ki = slider_ki.getValue()
pendulum.kd = slider_kd.getValue() # pendulum.kd = slider_kd.getValue()
# Update pendulum # Update pendulum
if not pendulum.fallen: if not pendulum.fallen:
@ -494,7 +489,6 @@
dt = clock.tick(60) # limits FPS to 120 dt = clock.tick(60) # limits FPS to 120
pygame.quit() pygame.quit()
\end{lstlisting} \end{lstlisting}
\subsection{pendulum.py} \subsection{pendulum.py}
\begin{lstlisting}[language=Python] \begin{lstlisting}[language=Python]
@ -510,6 +504,9 @@
C_P_ANG_START = 1 / 1000 * math.pi C_P_ANG_START = 1 / 1000 * math.pi
C_FALL_ANG = 52.5 / 100 * 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: class Pendulum:
def __init__(self, theta, length, dx, mass, color): def __init__(self, theta, length, dx, mass, color):
@ -536,6 +533,9 @@
self.a_ang = [0] # Angular acceleration self.a_ang = [0] # Angular acceleration
self.v_ang = [0] # Angular velocity self.v_ang = [0] # Angular velocity
## Gyro offset
self.theta_offset = 0
self.dx = dx # Horizontal displacement of "cart" from center self.dx = dx # Horizontal displacement of "cart" from center
self.a_cart = [0] # Acceleration of cart self.a_cart = [0] # Acceleration of cart
self.v_cart = [0] # Velocity of cart self.v_cart = [0] # Velocity of cart
@ -549,9 +549,16 @@
## PID variables ## PID variables
self.pid = True 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.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): def update(self, dt):
self.doMath(dt) self.doMath(dt)
@ -560,7 +567,12 @@
) )
if self.pid: 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: if abs(self.theta[self.index]) == C_FALL_ANG:
self.fallen = True self.fallen = True
@ -589,14 +601,14 @@
def doMath(self, dt): def doMath(self, dt):
### ANGLE ### ### ANGLE ###
ang_term1 = self.a_cart[self.index] * math.cos(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]) ang_term2 = self.v_cart[self.index] * math.sin(self.theta[self.index] + self.theta_offset)
ang_term3 = ( ang_term3 = (
self.v_cart[self.index] # Previous cart velocity self.v_cart[self.index] # Previous cart velocity
* self.v_ang[self.index] # previous angle 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 # Angular acceleration
self.a_ang.append( self.a_ang.append(
@ -610,10 +622,13 @@
) )
# Angular displacement # Angular displacement
self.theta.append( theta_res = self.theta[self.index] + (self.v_ang[self.index + 1] * (dt / 1000))
self.theta[self.index] # Previous angle theta_round = theta_res % C_ANG_RES_LIMIT
+ (self.v_ang[self.index + 1] * (dt / 1000)) 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 # Limit fall of pendulum
self.theta[self.index + 1] = self.clamp( self.theta[self.index + 1] = self.clamp(
@ -635,13 +650,20 @@
) )
# Cart acceleration # 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 # Integrate acceleration to get velocity
self.v_cart.append( v_res = self.v_cart[self.index] + (self.a_cart[self.index + 1] * (dt / 1000)) # Previous velocity
self.v_cart[self.index] # Previous velocity
+ (self.a_cart[self.index + 1] * (dt / 1000)) 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 # Cart displacement
self.s_cart.append( self.s_cart.append(
@ -667,6 +689,7 @@
self.a_cart = [0] self.a_cart = [0]
self.v_cart = [0] self.v_cart = [0]
self.s_cart = [0] self.s_cart = [0]
self.theta_offset = 0
self.fallen = False self.fallen = False
self.update(0) self.update(0)
@ -695,10 +718,14 @@
def pidControl(self, dt): def pidControl(self, dt):
error = self.theta[self.index] error = self.theta[self.index]
# (dt/1000) dt = (dt/1000)
# result = (self.kp * error) + (self.ki * sum(self.theta)) + (self.kd * self.v_ang[self.index]) #PID # 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 result = (self.kp * error) \
self.a_cart[self.index] = result * 10 + (((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} \end{lstlisting}
\subsection{uiHelpers.py} \subsection{uiHelpers.py}
\begin{lstlisting}[language=Python] \begin{lstlisting}[language=Python]
@ -821,7 +848,6 @@
self.blink = not self.blink self.blink = not self.blink
self.blinkTimer = 0 self.blinkTimer = 0
self.blinkTimer += dt self.blinkTimer += dt
\end{lstlisting} \end{lstlisting}
\newpage \newpage
\section{Control source code} \section{Control source code}
@ -837,18 +863,25 @@
from ev3dev2.sound import Sound from ev3dev2.sound import Sound
import time import time
import logging # import logging
import os import os
import csv
from datetime import datetime from datetime import datetime
now = datetime.now() # current date and time now = datetime.now() # current date and time
LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + ".csv")) # LOGNAME = os.path.join(os.getcwd(), "logs/", (now.strftime("%m-%d_%H:%M:%S") + "final.csv"))
# print(LOGNAME) # # print(LOGNAME)
logging.basicConfig(filename=LOGNAME, # logging.basicConfig(filename=LOGNAME,
filemode='a', # filemode='a',
format='%(message)s', # format='%(message)s',
level=logging.DEBUG) # level=logging.DEBUG)
log_t = []
log_ang = []
log_vang = []
log_x = []
log_v = []
# Motors # Motors
l_motor = LargeMotor(OUTPUT_B) l_motor = LargeMotor(OUTPUT_B)
@ -859,54 +892,90 @@
# Sound # Sound
sound = 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 # tires are 56mm diameter
def balance(target_angle=0, k_p=150, k_i=0, k_d=0.1): def balance(target_angle=0):
integrated_error = 0
t = time.time() 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 # Calibrate gyro in current position
reset_gyro() gyro.calibrate()
gyro.mode = GyroSensor.MODE_GYRO_G_A
angle = gyro.angle 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 # Stop if the robot has fallen over
while abs(angle) < 40: while abs(angle) < 40:
# Keep time # Keep time
dt = time.time() - t dt = time.time() - t
t = time.time() t = time.time()
sum_motor_pos = l_motor.position + r_motor.position
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
angle, rate = gyro.angle_and_rate angle, rate = gyro.angle_and_rate
log_t.append(t)
log_ang.append(angle)
log_vang.append(rate)
log_x.append(pos)
log_v.append(speed)
error = angle - target_angle error = angle - target_angle
integrated_error = integrated_error + error * dt pd = gyro_k_p * error + gyro_k_d * rate \
+ motor_k_p * pos + motor_k_d * speed
pid = k_p * error + k_i * integrated_error + k_d * rate # convert -100 ~ 100 to -1050 ~ 1050
speed = (((pd - (-100)) * (1049 - (-1049))) / (100 - (-100))) + (-1049)
# print(t, angle, integrated_error, rate, pid)
logging.debug("%d;%d;%d", angle, rate, pid)
if abs(pid) > 1050:
print("cap")
speed = min(max(pid, -1050), 1050)
l_motor.run_forever(speed_sp=speed) l_motor.run_forever(speed_sp=speed)
r_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") l_motor.stop(stop_action="hold")
r_motor.stop(stop_action="hold") r_motor.stop(stop_action="hold")
balance()
\end{lstlisting}
# 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 \end{document} % This is the end of the document