\documentclass[aspectratio=169]{beamer}
\usepackage{mum-theme-beamer/mum-theme}

\input{code-block-settings.tex}
\input{tikz-settings.tex}

\title{Formulas and Vehicles}
\subtitle{Implementation of a PID-Controller}
\date{03.11.2023}
\author[\textbf{L. Alff}, N. Bauschmann, D. Duecker]{Lennart Alff, Nathalie Bauschmann, Daniel Duecker}
\institute[TUHH]{Hamburg University of Technology}

\begin{document}

\begin{frame}
	\titlepage
\end{frame}

\begin{frame}
	\frametitle{How to Start?}
	\pause
	\textbf{\Large Starting from scratch\dots}\vskip0.5cm
	\pause
	\textbf{\Large Identify the challenges:}\vskip0.25cm
	\pause
	\begin{enumerate}
		\item Communication/data flow -- the ROS perspective \pause
		\item Implementing the control law -- the control perspective
	\end{enumerate}
\end{frame}

\begin{frame}
	\frametitle{ROS Graph}
	\centering
	\begin{tikzpicture}[node distance=10mm and 20mm]
		\node (baro) [rosnode, alt=<2>{red}{}, alt=<3->{gray}{}] {Barometer};
		\node (depth_calculator)
		[
			rosnode,
			right=of baro,
			alt=<4>{red}{},
			alt=<5->{gray}{},
            alt=<7->{mumgreen}{},
		] {Depth\\Calculator};
        \node (controller)[rosnode, right=of depth_calculator, alt=<6>{red}{}, alt=<7->{mumgreen}{}] {Controller};
        \node (setpoint_publisher)[rosnode, below=of depth_calculator, alt=<5>{red}{}, alt=<6>{gray}{}, alt=<7->{mumgreen}{}] {Setpoint\\Publisher};

		\draw[arrow] (baro) %
		edge node [sloped, anchor=center, above] {\footnotesize pressure} %
		node [sloped, anchor=center, below] {\tiny FluidPressure} %
		(depth_calculator);

		\draw[arrow] (depth_calculator)
		edge node [sloped, anchor=center, above] {\footnotesize depth}
		node [sloped, anchor=center, below] {\tiny DepthStamped}
		(controller);

		\draw[arrow] (setpoint_publisher)
		edge node [sloped, anchor=center, above, xshift=-5mm] {\footnotesize depth\_setpoint}
		node [sloped, anchor=center, below, xshift=-5mm] {\tiny Float64Stamped}
		(controller);

		\node (empty_right) [right=of controller] {};
		\draw[arrow] (controller)
		edge node [sloped, anchor=center, above] {\footnotesize thrust\_setpoint}
		node [sloped, anchor=center, below] {\tiny ActuatorSetpoint}
		(empty_right);

	\end{tikzpicture}
    \begin{itemize}
        \color{mumgreen}
        \item<7-> these nodes are included in our template for assignment 1 \pause
        \item<8-> communication between them is already set up! 
    \end{itemize}
\end{frame}

\begin{frame}[fragile]
	\frametitle{Where to Start?}
	% \begin{noindent}
    \begin{pythoncode*}{label=depth\_controller.py}
class DepthControlNode(Node):

    def __init__(self):
        super().__init__(node_name='depth_controller')

        self.thrust_pub = self.create_publisher(ActuatorSetpoint,
                                                'thrust_setpoint', 1)

        self.setpoint_sub = self.create_subscription(Float64Stamped,
                                                     'depth_setpoint',
                                                     self.on_setpoint, 1)
        self.depth_sub = self.create_subscription(DepthStamped, 'depth',
                                                  self.on_depth, 1)

    |\vdots|
	\end{pythoncode*}
    % \end{noindent}
	\begin{itemize}
		\item<2-> \color<4->{gray}{implement callbacks (\texttt{on\_setpoint}, \texttt{on\_depth})} \only<4->{\color{mumbluefont}{$\Rightarrow$ given in template!}}
		\item<3-> \color<5->{red}{implement the control law}
	\end{itemize}
\end{frame}


\begin{frame}[fragile]
	\frametitle{Not Yet a PID Controller}
	\textbf{\Large Again, starting point already provided in our template!}
	\vskip0.5cm
	% \begin{noindent}
        \begin{pythoncode}
class DepthControlNode(Node):
        |\vdots|

    def compute_control_output(self, current_depth: float) -> float:
        thrust = current_depth
        return thrust
	\end{pythoncode}
    % \end{noindent}
\end{frame}

\begin{frame}
    \frametitle{What was PID Control Again?}
    \textbf{\Large Control law:}\vskip0.25cm
    \begin{equation}
        u = K_{\mathrm{p}} e + K_\mathrm{i} \int_0^{t} e\,\mathrm{d}\tau + K_{\mathrm{d}}\dot{e}
    \end{equation}
    \pause
    \centering
    \begin{tikzpicture}[node distance=10mm and 20mm]
       \coordinate (start);
        \coordinate[right=of start] (first_junction);
        \node (integral)[right=of first_junction, rostopic]{$K_{\mathrm{i}}\int (\cdot)$};
        \node (proportional)[above=of integral, rostopic]{$K_{\mathrm{p}}$};
        \node (derivative)[below=of integral, rostopic]{$K_{\mathrm{d}}\frac{\mathrm{d}}{\mathrm{d}t}$};
        %\coordinate[right=of integral] (second_junction);
        \node (second_junction)[right=of integral, draw, circle]{$\sum$};
        \coordinate[right=of second_junction] (end);

        \draw[] (start) edge node[above]{$e$} (first_junction);
        \draw (first_junction) |- (proportional);
        \draw (first_junction) |- (integral);
        \draw (first_junction) |- (derivative);
        \draw (proportional) -| (second_junction);
        \draw (integral) -- (second_junction);
        \draw (derivative) -| (second_junction);
        \draw (second_junction) edge node[above]{$u$} (end);
   \end{tikzpicture} 

\end{frame}

\begin{frame}[fragile]
	\frametitle{Proportional Control}
	\textbf{\Large Control law:}\vskip0.25cm
	\begin{equation}
		u = \textcolor{red}{K_{\mathrm{p}} e} + K_\mathrm{i} \int_0^{t} e\,\mathrm{d}\tau + K_{\mathrm{d}}\dot{e}
	\end{equation}
    \pause
	\textbf{\Large Code:}\vskip0.25cm
	% \begin{noindent}
        \begin{pythoncode}
class DepthControlNode(Node):
        |\vdots|

    def compute_control_output(self, current_depth: float) -> float:
        p_gain = 1.0
        error =  current_depth - self.current_setpoint
        thrust = p_gain * error
        return thrust
	\end{pythoncode}
    % \end{noindent}
\end{frame}

\begin{frame}[fragile]
	\frametitle{PI-Control}
	\begin{equation}
		u = K_{\mathrm{p}} e + \textcolor{mumblue}{K_\mathrm{i} \int_0^{t} e\,\mathrm{d}\tau} + K_{\mathrm{d}}\dot{e}
	\end{equation}
    \pause
	% \begin{noindent}
        \begin{pythoncode}
class DepthControlNode(Node):
    def __init__(self):
        self.error_integral = 0.0
        self.last_time = self.get_clock().now().nanoseconds * 1e-9
        |\dots|

    def compute_control_output(self, current_depth: float) -> float:
        p_gain = 1.0
        i_gain = 1.0
        error =  current_depth - self.current_setpoint

        now = self.get_clock().now().nanoseconds * 1e-9
        dt = now - self.last_time
        
        self.error_integral = self.error_integral + dt * error

        thrust = p_gain * error + i_gain * self.error_integral

        self.last_time = now
        return thrust
	\end{pythoncode}
    % \end{noindent}
\end{frame}

\begin{frame}[fragile]
	\frametitle{PID-Control}
	\alt<1>{
		\begin{equation}
			u = K_{\mathrm{p}} e + K_\mathrm{i} \int_0^{t} e\,\mathrm{d}\tau + \textcolor{mumgreen}{K_{\mathrm{d}}\dot{e}}
		\end{equation}
	}{}
	\pause
	% \begin{noindent}
        \begin{pythoncode}
class DepthControlNode(Node):
    def __init__(self):
        self.error_integral = 0.0
        self.last_time = self.get_clock().now().nanoseconds * 1e-9
        self.last_error = 0.0
        |\dots|

    def compute_control_output(self, current_depth: float) -> float:
        p_gain = 1.0
        i_gain = 1.0
        d_gain = 1.0

        error =  current_depth - self.current_setpoint

        now = self.get_clock().now().nanoseconds * 1e-9
        dt = now - self.last_time 

        self.error_integral = self.error_integral + dt * error

        derror = (error - self._last_error) / dt

        thrust = p_gain * error + i_gain * self.error_integral + d_gain * derror

        self.last_time = now
        self.last_error = error
        return thrust
	\end{pythoncode}
    % \end{noindent}
\end{frame}

\begin{frame}
    \frametitle{See the Controller in Action}
    \centering
    \textbf{\Large Time for the fun part?}
\end{frame}

\end{document}