%
% PIDPOS.tex
%

\documentclass{beamer}

\usepackage{beamerthemeshadow}
\usepackage{listings}
\usepackage{graphics}
\usepackage{times}
\usepackage{algorithm2e}

\usepackage{pgf,pgfarrows,pgfnodes,pgfautomata,pgfheaps,pgfshade}
% \usepackage{amsmath,amssymb} \usepackage[latin1]{inputenc}
% \usepackage{colortbl}
\usepackage[english]{babel}

\newcommand{\erlang}{
\lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL,
  frameround=fttt}}

\newcommand{\erlangsmall}{
\lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL,
  frameround=fttt}}

\newcommand{\clang}{
\lstset{language=Prolog, basicstyle=\ttfamily\footnotesize,frame=trBL,
  frameround=fttt}}

\newcommand{\clangsmall}{
\lstset{language=Prolog, basicstyle=\ttfamily\scriptsize,frame=trBL,
  frameround=fttt}}

\newcommand{\clangtiny}{
\lstset{language=Prolog, basicstyle=\ttfamily\tiny,frame=trBL,
  frameround=fttt}}

\erlang

\newcommand\blue{\color[rgb]{0,0,1}}
\newcommand\green{\color[rgb]{0,0.5,0}}
\newcommand\red{\color[rgb]{1,0,0}}
\newcommand\black{\color[rgb]{0,0,0}}

\newcommand\tape[9]{
\framebox{#1}
\framebox{#2}
\framebox{#3}
\framebox{#4}
\framebox{#5}
\framebox{#6}
\framebox{#7}
\framebox{#8}
\framebox{#9}}

\newcommand\two[2]{
\framebox{#1}
\framebox{#2}}

\newcommand\six[6]{
\framebox{#1}
\framebox{#2}
\framebox{#3}
\framebox{#4}
\framebox{#5}
\framebox{#6}}

\newcommand\three[3]{
\framebox{#1}
\framebox{#2}
\framebox{#3}}

\title[Controllo PID in posizione]{Controllo PID in posizione}
%%

\author[Corrado Santoro]
       {Corrado Santoro}

\date{ }

\institute[University of Catania]
  {
    \begin{center}
      \textbf{\red{}ARSLAB - Autonomous and Robotic Systems Laboratory}\\
      Dipartimento di Matematica e Informatica - Universit\`{a} di Catania, Italy\\
      \texttt{santoro@dmi.unict.it}\\
      \includegraphics[width=60pt]{robotEscher_Eura_blur.eps}\\%%~~~~\includegraphics[width=80pt]{logo_dmi.png}
      Programmazione Sistemi Robotici\\
    \end{center}
  }



\begin{document}

\frame{\titlepage}


%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in velocit\`{a} di una massa}
%%
\begin{center}
\includegraphics[scale=0.2]{massa_attrito_pid_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Consideriamo il  classico esempio di controllo in velocit\`{a}
  di massa su piano con attrito
%%
\item Includiamo l'intero sistema in una ``black box'' (che chiamiamo
  $S+M$, \emph{speed control+ mass}) che ha come input
  una \textbf{velocit\`{a} target} e output la \textbf{\red{}velocit\`{a}
    effettiva}
%%
\item Compito di tale sistema \`{e} assicurare che la velocit\`{a} effettiva
  segua sempre la velocit\`{a} target
%%
\item Ci\`{o} \`{e} assicurato dal PID presente dentro il blocco $S+M$
%%
\end{itemize}
%%
%%
\end{frame}




%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in posizione di una massa}
%%
\begin{center}
\includegraphics[scale=0.2]{massa_attrito_3.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Adesso supponiamo che desideriamo far s\`{i} che la
  nostra massa raggiunga una certa \textbf{posizione lineare} $\overline{x}$
%%
\item Avendo a disposizione il \textbf{controllo in velocit\`{a}} possiamo:
  \begin{itemize}
  \item\footnotesize{}\red{}Misurare la posizione corrente $p(t)$ (integrale della
    velocit\`{a})
  \item\red{}Confrontarla con la nostra posizione target $\overline{x}$
  \item\red{}Usare un \textbf{PID} che, sulla base dell'errore $e(t) = \overline{x} -
    p(t)$ produce la \textbf{velocit\`{a} target} che la nostra massa deve
    avere
  \end{itemize}
%%
\end{itemize}
%%
%%
\end{frame}





%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in posizione di una massa}
%%
\begin{itemize}
%%
\item\footnotesize{}Lo schema di controllo diventa il seguente:
\begin{center}
\includegraphics[scale=0.2]{massa_posizione.eps}
\end{center}
~\\
\item\footnotesize{}ed ``esplodendo'' il blocco $S+M$ abbiamo:
\end{itemize}
\begin{center}
\includegraphics[scale=0.2]{massa_posizione_2.eps}
\end{center}
%%
%%
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Controllore di posizione}
%%
\begin{center}
\includegraphics[scale=0.2]{massa_posizione_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Il comportamento del controllore in posizione pu\`{o}
  essere immaginato secondo due opzioni:
%%
  \begin{itemize}
  \item\footnotesize{}\red{}un \textbf{PID} a tutti gli effetti;
  \item\footnotesize{}\red{}uno specifico algoritmo che genera un
    \textbf{profilo di velocit\`{a}}.
  \end{itemize}
\end{itemize}
%%
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo di posizione tramite PID}
%%
\begin{center}
\includegraphics[scale=0.2]{massa_posizione_3.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Possiamo immaginare il seguente comportamento:
%%
  \begin{itemize}
    %%
  \item\footnotesize{}Se siamo sul target ($\red{}errore=0$) la nostra
    velocit\`{a} sar\`{a} \textbf{nulla}
    %%
  \item Se siamo lontani dal target ($\red{}errore \neq 0$) la nostra
    velocit\`{a} sar\`{a} \textbf{\red{}tanto pi\`{u} elevata quanto
      pi\`{u} lontani} siamo dal target, fino ad un \textbf{limite massimo}
    %%
  \end{itemize}
%%
\item{\red{}questa tipologia di controllo implica un semplice
  \textbf{controllore proporzionale con saturazione}}
%%
\item La \textbf{saturazione} \`{e} data dalla velocit\`{a} massima $VMAX$
%%
\item Il valore di $\red{}K_p$ determina ``quanto velocemente'' vogliamo
  avvicinarci al target
%%
\end{itemize}
%%
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della Velocit\`{a}}
%%
\begin{center}
$\red{}K_p = 0.5$\\
\includegraphics[scale=0.4]{pos_trend_speed.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La velocit\`{a} \`{e} massima durante la ``marcia'' e
  scende gradualmente man mano che ci si avvicina alla posizione desiderata
\end{itemize}
%%
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della Posizione}
%%
\begin{center}
$\red{}K_p = 0.5$\\
\includegraphics[scale=0.4]{pos_trend_pos.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La massa si avvicina gradualmente alla posizione target
  (10 m) e poi si ferma l\`{i}
\end{itemize}
%%
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della Velocit\`{a}}
%%
\begin{center}
$\red{}K_p = 2.5$\\
\includegraphics[scale=0.4]{pos_trend_speed_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Se il valore di $K_p$ \`{e} troppo elevato, l'effetto
  che si ottiene \`{e} un \textbf{superamento} del punto target, per poi
  ``tornare indietro''
\end{itemize}
%%
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della Posizione}
%%
\begin{center}
$\red{}K_p = 2.5$\\
\includegraphics[scale=0.4]{pos_trend_pos_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Nell'andamento della posizione compare dunque una
  \textbf{sovraelongazione}
%%
\end{itemize}
%%
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione del controllo in posizione}
%%
\begin{block}{}
\Large{}Implementazione del controllo in posizione tramite profilo di velocit\`{a}
\end{block}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Controllore di posizione}
%%
\begin{center}
\includegraphics[scale=0.2]{massa_posizione_4.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La seconda tecnica di controllo prevede l'uso di un
  \textbf{algoritmo software} che genera il \textbf{\red{}profilo di
    velocit\`{a}} da seguire per poter raggiungere la posizione target.
%%
\end{itemize}
%%
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Profilo di Velocit\`{a}}
%%
\begin{center}
\includegraphics[scale=0.3]{speed_profile.eps}
\end{center}
%%
\begin{itemize}
\item\footnotesize{}I tipici profili di velocit\`{a} dei sistemi meccanici
  prevedono tre fasi:
%%
  \begin{enumerate}
    %%
  \item\footnotesize{}Una prima fase di \textbf{\red{}accelerazione},
    parametro $a_1 = cost$
    %%
  \item Una fase di \emph{crociera} a \textbf{\red{}velocit\`{a} costante},
    $a_2 = 0, v = V_{max}$
    %%
  \item Una fase di \textbf{\red{}decelerazione}, con parametro $a_3 = cost$
    %%
  \end{enumerate}
%%
\item L'obiettivo \`{e} far s\`{i} che, al termine della fase di
  decelerazione, la massa da controllare si trovi effettivamente alla
  \textbf{posizione target}
%%
\item Il controllore \`{e} totalmente determinato dai \textbf{parametri}
  $\red{}a_1$, $\red{}V_{max}$ e $\red{}a_3$, pi\`{u} il tempo di
  campionamento $\red{}T_{camp}$.
%%
\end{itemize}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Profilo di Velocit\`{a} e Moto uniformemente accelerato}
%%
Per determinare l'algoritmo del controllore, useremo le formule del
\textbf{moto uniformemente accelerato} che qui ricordiamo:
%%
\begin{eqnarray*}
a &= &cost\\
v(t) & = & v(0) + a t\\
x(t) & = & x(0) + v(0) t + \frac{1}{2} a t^2
\end{eqnarray*}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Prima definizione della classe}
%%
%%
\footnotesize{}Implementeremo il controllore non pi\`{u} usando la classe
\texttt{Pid}, ma una una classe ad-hoc che chiameremo
\texttt{\textbf{PositionController}}:
%%
\black\clangtiny%%
\begin{lstlisting}
class PositionController {
 public:
    PositionController(float accel, float v_max, float decel, float dt);
    float evaluate(float target, float current_pos, float current_speed);
 protected:
    float m_accel; // accelerazione
    float m_vmax;  // vel max
    float m_decel; // decelerazione
    float m_dt;	   // tempo di campionamento

    float m_next_speed; // velocity

    float m_accel_step;
    float m_decel_distance;
};

\end{lstlisting}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione delle fasi}
%%
\begin{center}
\includegraphics[scale=0.25]{speed_profile.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Le fasi {\red{}1} e {\red{}2} sono abbastanza semplici:
si tratta di incrementare la velocit\`{a}, ad ogni step di simulazione, del
valore $a_1 T_{camp}$, fino alla saturazione $V_{max}$:
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
float PositionController::evaluate(float target, float current_pos, float current_speed)
{
   ....
        // Fase 1, fase 2
        if (m_next_speed < m_vmax) {
            m_next_speed += m_accel * m_dt; // incremento
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
    ....
    return m_next_speed;
}
\end{lstlisting}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione delle fasi}
%%
\begin{center}
\includegraphics[scale=0.25]{speed_profile.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Per implemntare la fase {\red{}3} \`{e} necessario
  identificare il punto in cui la fase 3 stessa \textbf{deve cominiciare}
%%
\item Poich\`{e} possiamo conoscere la \textbf{distanza} (errore) tra la
  posizione corrente $x(t)$ e la posizione target $d = X_{target} - x(t)$,
  possiamo usare questa informazione per calcolare \textbf{a quale distanza
    deve iniziare la decelerazione}
%%
\item Determiniamo quindi la \textbf{\red{}distanza di decelerazione}
%%
\end{itemize}
%%
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{center}
\includegraphics[scale=0.2]{speed_profile_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Operiamo una \textbf{traslazione temporale} e
  consideriamo il tempo $0$ l'inizio della fase 3
%%
\item Usando la formula del moto uniformemente accelerato, calcoliamo il
  \textbf{\red{}tempo necessario per arrivare a velocit\`{a} zero} (tempo
  di decelerazione) $T_D$:
%%
\end{itemize}
%%
\begin{eqnarray*}
v(T_D) & = & v(0) + a_3 T_D\\
0 & = & V_{max} + a_3 T_D\\
\red{}T_D & \red{}= & \red{}- \frac{V_{max}}{a_3}
\end{eqnarray*}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{center}
\includegraphics[scale=0.2]{speed_profile_2.eps}
\end{center}
%%
\begin{eqnarray*}
v(T_D) & = & v(0) + a_3 T_D\\
0 & = & V_{max} + a_3 T_D\\
\red{}T_D & \red{}= & \red{}- \frac{V_{max}}{a_3}
\end{eqnarray*}
%%
\begin{itemize}
%%
\item\footnotesize{}Il segno ``meno'' \`{e} normale perch\`{e}, essendo
  $a_3$ \textbf{negativo} (\`{e} una decelerazione), accade che $T_D$
  risulti alla fine \textbf{\red{}positivo}.
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
\begin{itemize}
%%
\item\footnotesize{}Determiniamo a questo punto la \textbf{distanza di
  decelerazione} $X_D = x(T_D)$:
%%
\end{itemize}
%%
\begin{eqnarray*}
x(T_D) & = & x(0) + v(0) T_D + \frac{1}{2} a_3 T_D^2\\
x(T_D) & = & 0 +V_{max} T_D + \frac{1}{2} a_3 T_D^2\\
x(T_D) & = & V_{max} ( - \frac{V_{max}}{a_3}) + \frac{1}{2} a_3 ( - \frac{V_{max}}{a_3})^2\\
x(T_D) & = &  - \frac{V_{max}^2}{a_3} + \frac{1}{2} a_3 \frac{V_{max}^2}{a_3^2}\\
x(T_D) & = &  - \frac{V_{max}^2}{a_3} + \frac{1}{2}\frac{V_{max}^2}{a_3}\\
\red{}X_D & \red{}= &  \red{}- \frac{V_{max}^2}{2 a_3}\\
\end{eqnarray*}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione delle fasi}
%%
\begin{itemize}
%%
\item\footnotesize{}Se la distanza dal target \`{e} inferiore a $X_D$
  allora siamo nella fase di decelerazione. Il codice diventa:
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
PositionController::PositionController(float accel, float v_max, float decel, float dt)
    : m_accel(accel), m_vmax(v_max), m_decel(decel), m_dt(dt), m_next_speed(0)
{
    m_accel_step = m_accel * m_dt;
    m_decel_distance = (m_vmax * v_max) / (2 * m_decel); // si considera decel positiva
}

float PositionController::evaluate(float target, float current_pos, float current_speed)
{
    float distance = target - current_pos;
    if (distance < m_decel_distance) {
        // fase 3
        ....
    }
    else {
        // Fase 1, fase 2
        if (m_next_speed < m_vmax) {
            m_next_speed += m_accel * m_dt; // incremento
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
    }
    return m_next_speed;
}
\end{lstlisting}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{center}
\includegraphics[scale=0.2]{speed_profile.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Durante la fase 3, il controllore deve fornire la
  velocit\`{a} alla quale la massa deve andare
%%
\item Poich\`{e} conosciamo la distanza  tra {\red{}posizione
  target} e {\red{}posizione corrente} usiamo questa informazione per
  determinare la  velocit\`{a} zero
%%
\item Dobbiamo quindi trovare una funzione del tipo
\[\red{}
 v(t) = f(e(t)), e(t) = X_{target} - x(t)
\]
%%
\item A tale scopo usiamo ancora una volta le equazioni della cinematica
  applicate alla \textbf{sola fase di decelerazione}
%%
\end{itemize}
%%
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{itemize}
\item\footnotesize{}Consideriamo il tempo $0$ l'inizio della fase della
  decelerazione, quindi $v(0) = V_{max}$ e $x(0) = 0$, abbiamo:
%%
\begin{eqnarray*}
v(t) & = & V_{max} + a_3 t\\
x(t) & = & V_{max}t + \frac{1}{2}a_3 t^2
\end{eqnarray*}
%%
\item Poniamo, per brevit\`{a}, $x = x(t)$ e $v = v(t)$ e calcoliamo il tempo
  dalla prima equazione:
\begin{eqnarray*}
t & = & \frac{v - V_{max}}{a_3}
\end{eqnarray*}
%%
\item e sostituiamolo nella seconda:
\end{itemize}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{eqnarray*}
x & = & V_{max}\frac{v - V_{max}}{a_3} + \frac{1}{2}a_3 (\frac{v -  V_{max}}{a_3})^2\\
x & = & \frac{V_{max}v - V_{max}^2}{a_3} + \frac{v^2 -2 v V_{max} +  V_{max}^2}{2 a_3}\\
2 a_3 x & = & 2 V_{max}v - 2 V_{max}^2 + v^2 -2 V_{max}v +  V_{max}^2\\
2 a_3 x & = & - 2 V_{max}^2 + v^2  +  V_{max}^2\\
2 a_3 x & = & v^2  -  V_{max}^2\\
\red{}v & \red{}= & \red{}\sqrt{V_{max}^2 + 2 a_3 x}
\end{eqnarray*}
%%
\begin{itemize}
\item\footnotesize{}$x$ \`{e} la distanza percorsa a partire
  \textbf{dall'inizio della fase di decelerazione}
\item Tuttavia noi possediamo la \textbf{distanza dal target}, quindi ...
\end{itemize}
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione della fase 3}
%%
\begin{eqnarray*}
v & = & \sqrt{V_{max}^2 + 2 a_3 x}\\
\end{eqnarray*}
%%
\begin{center}
\includegraphics[scale=0.2]{pos_fase_3.eps}
\end{center}
%%
\begin{itemize}
\item\footnotesize{}$x$ \`{e} pari alla \textbf{distanza di decelerazione}
  $X_D$ meno la \textbf{distanza dal target}:
\[
x = X_D - (target\_pos - current\_pos)
\]
\item pertanto ...
\begin{eqnarray*}
\red{}v & \red{}= & \red{}\sqrt{V_{max}^2 + 2 a_3 (X_D - (target\_pos - current\_pos))}
\end{eqnarray*}
\item se $a_3$ la si considera in valore assoluto abbiamo:
\begin{eqnarray*}
\red{}v & \red{}= & \red{}\sqrt{V_{max}^2 - 2 a_3 (X_D - (target\_pos - current\_pos))}
\end{eqnarray*}
%%
\end{itemize}
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione delle fasi}
%%
\begin{itemize}
%%
\item\footnotesize{}Il codice che include anche la fase 3 diventa:
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
PositionController::PositionController(float accel, float v_max, float decel, float dt)
    : m_accel(accel), m_vmax(v_max), m_decel(decel), m_dt(dt), m_next_speed(0)
{
    m_accel_step = m_accel * m_dt;
    m_decel_distance = (m_vmax * v_max) / (2 * m_decel); // si considera decel positiva
}

float PositionController::evaluate(float target, float current_pos, float current_speed)
{
    float distance = target - current_pos;
    if (distance < m_decel_distance) {
        // fase 3
        float expected_speed = sqrt(m_vmax * m_vmax -
                                    2 * m_decel * (m_decel_distance - distance));
        m_next_speed = expected_speed;
    }
    else {
        // Fase 1, fase 2
        if (m_next_speed < m_vmax) {
            m_next_speed += m_accel * m_dt; // incremento
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
    }
    return m_next_speed;
}
\end{lstlisting}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della velocit\`{a}}
%%
L'andamento della velocit\`{a}, con questo tipo di controllo, ed una
distanza di $8m$, \`{e}
\begin{center}
\red{}$a_1 = 0.3 m/s^2, V_{max} = 2 m/s, a_3 = 0.2 m/s^2$\\
\includegraphics[scale=0.35]{plot_vel_trapezio.eps}
\end{center}
%%
%%
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Andamento della distanza}
%%
L'andamento della distanza, con questo tipo di controllo, ed una
distanza da percorrere di $8m$, \`{e}
\begin{center}
\red{}$a_1 = 0.3 m/s^2, V_{max} = 2 m/s, a_3 = 0.2 m/s^2$\\
\includegraphics[scale=0.35]{plot_distanza.eps}
\end{center}
%%
%%
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Fasi e Durata}
%%
\begin{itemize}
\item\footnotesize{}La durata delle fasi di accelerazione e decelerazione
  \`{e} funzione diretta delle costanti di accelerazione e decelerazione
\item Una volta impostata  $a_1$ e $a_3$, tali durate sono \textbf{sempre uguali}
\item Al variare della distanza da percorrere, varier\`{a} pertanto la
  durata della fase a velocit\`{a} costante
\end{itemize}
%%
\begin{center}
\red{}$target = 8m$~~~~~~~~~~~~~~~~~~~~~~~~$target = 5m$\\
\includegraphics[scale=0.25]{plot_vel_trapezio.eps}~~
\includegraphics[scale=0.25]{plot_vel_trapezio_2.eps}
\end{center}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Fasi e Durata}
%%
\begin{itemize}
\item\footnotesize{}La fase a velocit\`{a} costante potrebbe addirittura
  sparire nel caso la distanza sia particolarmente ``corta''
\item In tal caso il trapezio degenera in un \textbf{triangolo}
\end{itemize}
%%
\begin{center}
\red{}$target = 4m$\\
\includegraphics[scale=0.35]{plot_vel_triangolo.eps}
\end{center}
%%
\end{frame}
%%






%%
\begin{frame}[fragile]
%%
  \frametitle{Degenerazione del trapezio e discontinuit\`{a}}
%%
\begin{itemize}
\item\footnotesize{}Tuttavia se distanza \`{e} ulteriormente ``corta'', le
  fasi 1 e 3 si \emph{accavallano}
\item In tal caso, l'algortimo descritto presenta un fastidioso punto di
  discontinuit\`{a} e il comportamento diventa il seguente:
%%
\begin{center}
\red{}$target = 3m$\\
\includegraphics[scale=0.3]{plot_vel_degenere.eps}
\end{center}
\item Il problema \`{e} dato dal fatto che siamo gi\`{a} entrati nella
  \textbf{distanza di decelerazione} senza aver \textbf{finito la fase di accelerazione}
\item La soluzione consiste nel \textbf{prolungare la fase di
  accelerazione} fin quando il segmento crescente non \textbf{incontra} il
  tratto di decelerazione
\end{itemize}
%%
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Soluzione del problema della discontinuit\`{a}}
%%
\begin{center}
\red{}$target = 3m$\\
\includegraphics[scale=0.25]{plot_vel_degenere.eps}
\end{center}
\begin{itemize}
%%
\item\footnotesize{}La fase 3 deve essere caratterizzata da una
  \textbf{invariabilit\`{a}}: \textbf{\red{}la velocit\`{a} da raggiungere deve
    essere minore della velocit\`{a} corrente}, altrimenti non ha senso
  parlare di \emph{decelerazione}
%%
\item Nel caso in figura accade proprio il contrario: quando inizia la fase
  di decelerazione, la velocit\`{a} calcolata \textbf{\`{e} maggiore} della
  velocit\`{a} corrente
\item Possiamo pertanto sfruttare questa propriet\`{a} per ``far
  continuare'' l'accelerazione fino all'incontro con il segmento di decelerazione
\end{itemize}
%%
%%
\end{frame}
%%






%%
\begin{frame}[fragile]
%%
  \frametitle{Soluzione del problema della discontinuit\`{a}}
%%
\begin{itemize}
%%
\item\footnotesize{}Aggiungiamo questo caso particolare...
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
float PositionController::evaluate(float target, float current_pos, float current_speed)
{
    float distance = target - current_pos;
    if (distance < m_decel_distance) {
        // fase 3
        float expected_speed = sqrt(m_vmax * m_vmax -
                                    2 * m_decel * (m_decel_distance - distance));
        if (expected_speed > current_speed) {
            // la vel da raggiungere risulta maggiore della vel corrente
            // siamo ancora nella fase di accelerazione, pertanto
            // continuiamo ad accelerare
            m_next_speed += m_accel * m_dt; // incremento
            // tuttavia verifichiamo di non aver raggiunto la vel calcolata
            if (m_next_speed > expected_speed) m_next_speed = expected_speed;
            // o di superare la vel massima
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
        else // qui siamo regolarmente nella fase di decelerazione
            m_next_speed = expected_speed;
    }
    else {
        // Fase 1, fase 2
        if (m_next_speed < m_vmax) {
            m_next_speed += m_accel * m_dt; // incremento
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
    }
    return m_next_speed;
}
\end{lstlisting}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione finale}
%%
\begin{itemize}
%%
\item\footnotesize{}A questo punto il codice va generalizzato tenendo in
  considerazione distanze \textbf{positive} e distanze \textbf{negative}
%%
\item Il segno della distanza implica un segno analogo nella velocit\`{a}
  risultante
%%
\item Pertanto, mettiamo da parte il segno, lavoriamo con i valori
  assoluti, e poi consideriamo il segno al momento di restituire la
  velocit\`{a} effettiva
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
float PositionController::evaluate(float target, float current_pos, float current_speed)
{
    float distance = target - current_pos;
    float s;

    if (distance < 0) {
        s = -1;
        distance = -distance;
    }
    else
        s = 1;
    ....
}
\end{lstlisting}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Implementazione finale}
%%
%%
\black\clangtiny%%
\begin{lstlisting}
float PositionController::evaluate(float target, float current_pos, float current_speed)
{
    ....
    if (distance < m_decel_distance) {
        // fase 3
        float expected_speed = sqrt(m_vmax * m_vmax -
                                    2 * m_decel * (m_decel_distance - distance));
        if (expected_speed > fabs(current_speed)) {
            // la vel da raggiungere risulta MAGGIORE della vel corrente
            // questo vuol dire che siamo ancora nella fase di accelerazione
            // pertanto acceleriamo
            m_next_speed += m_accel_step;
            if (m_next_speed > expected_speed) m_next_speed = expected_speed;
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
        else {
            m_next_speed = expected_speed;
        }
    }
    else {
        // Fase 1, fase 2
        if (m_next_speed < m_vmax) {
            m_next_speed += m_accel_step;
            if (m_next_speed > m_vmax) m_next_speed = m_vmax;
        }
    }
    return s*m_next_speed;
}
\end{lstlisting}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Test con distanza corta}
%%
\begin{itemize}
\item\footnotesize{}A questo punto, anche sulle distanze ``corte'',
  l'algoritmo funziona correttamente:
\end{itemize}
%%
\begin{center}
\red{}$target = 3m$\\
\includegraphics[scale=0.35]{plot_triangolo_corretto.eps}
\end{center}
%%
\end{frame}
%%



%%
\frame{\titlepage}
%%






\end{document}
%%
%%