%
% QuadControl.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[Architettura software del Motion Control]{Architettura Software del Motion
  Control}
%%

\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{Class Diagram}
%%
\begin{center}
\includegraphics[scale=0.3]{motion_class_diagram.eps}
\end{center}
%%
\begin{itemize}
\item\footnotesize{}Il software del motion control e basato su un insieme di \textbf{task periodici}
ognuno dei quali effettua le attivita di gestione e controllo della
movimentazione.
\item In pi\`{u} sono presenti alcune classi che rappresentano informazioni
aggiuntive sullo stato del robot
\end{itemize}
%%
\end{frame}


%%
\begin{frame}[fragile]
%%
  \frametitle{Classe Pose}
%%
\footnotesize{}La classe \texttt{\textbf{\red{}Pose}}, definita nel file
\texttt{geometry.h}, rappresenta le informazioni sulla \textbf{posizione}
del robot. Essa \`{e} derivata dalla classe pi\`{u} generica
\texttt{\textbf{\red{}Point}}.
%%
\black\clangtiny%%
\begin{lstlisting}
class Point {
 public:
   Point(float x = 0, float y = 0);
    ~Point() {};
    float x() { return m_x; };
    float y() { return m_y; };
    void x(float _x) { m_x = _x; };
    void y(float _y) { m_y = _y; };
    void operator+=(Point & p) { m_x += p.m_x; m_y += p.m_y;};
 private:
    float m_x, m_y;
};

class Pose : public Point {
 public:
 Pose(float x = 0, float y = 0, float theta = 0);
    float theta() { return m_theta;};
    void theta(float t) { m_theta = t; };
    void local_to_global(Point & robot_local, Point & global_point);
    void global_to_local(Point & global_point, Point & robot_local);
 private:
    float m_theta;
};
\end{lstlisting}
%%
\end{frame}
%%


%%
\begin{frame}[fragile]
%%
  \frametitle{Classe Pose. Metodi.}
%%
\begin{itemize}
%%
\item\footnotesize{}\texttt{\textbf{float x();}}
\item\texttt{\textbf{float y();}}
\item\texttt{\textbf{float theta();}}
  \begin{itemize}
  \item\footnotesize\red{}Restituisce la rispettiva coordinata
  \end{itemize}~\\
%%
\item\footnotesize{}\texttt{\textbf{void x(float new\_x);}}
\item\texttt{\textbf{void y(float new\_y);}}
\item\texttt{\textbf{void theta(float new\_theta);}}
  \begin{itemize}
  \item\footnotesize\red{}Imposta la rispettiva coordinata
  \end{itemize}~\\
%%
\item\texttt{\textbf{void operator+=(Point \& p);}}
  \begin{itemize}
  \item\footnotesize\red{}Somma tra punti
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Classe Pose. Metodi.}
%%
\begin{itemize}
%%
\item\footnotesize{}\texttt{\textbf{void local\_to\_global(Point \&
    robot\_local, \\~~~~~~~~~~~~~~~~~~~~~Point \& global\_point);}}
  \begin{itemize}
  \item\footnotesize\red{}Converte un punto da coordinate locali del robot
    a globali
  \end{itemize}~\\
%%
\item\footnotesize{}\texttt{\textbf{void global\_to\_local(Point \&
    global\_point, \\~~~~~~~~~~~~~~~~~~~~~Point \& robot\_local);}}
  \begin{itemize}
  \item\footnotesize\red{}Converte un punto da coordinate globali a locali
    del robot
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica di Robot Mobile su Ruote}
%%
\begin{center}
\includegraphics[scale=0.3]{reference_system.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La \textbf{\red{}posa} istantanea di un robot su un
  ambiente \textbf{bidimensionale} \`{e} caratterizzata da:
%%
  \begin{itemize}
    %%
  \item\red\footnotesize{}Posizione $x$
  \item Posizione $y$
  \item Orientamento $theta$ (rispetto all'asse $x$)
    %%
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica di Robot Mobile su Ruote}
%%
\begin{center}
\includegraphics[scale=0.3]{reference_system_2.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Si considerano due sistemi di riferimento:
  \begin{itemize}
    %%
  \item\footnotesize\textbf{\red{}Body system}, sistema di riferimento
    \emph{locale} del robot $(x',y')$
  \item\textbf{\red{}Earth system}, sistema di riferimento
    \emph{globale} dell'ambiente $(x,y)$
    %%
  \end{itemize}
%%
\item La trasformazione tra un sistema e l'altro avviene usando le normali
  formule di roto-traslazione, implementate nei metodi
  \texttt{\textbf{global\_to\_local}} e \texttt{\textbf{local\_to\_global}} della classe
  \texttt{\textbf{Pose}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Modello di Robot Mobile a due ruote indipendenti}
%%
\begin{center}
\includegraphics[scale=0.3]{robot_model.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}Due ruote indipendent poste ai lati del robot
\item Il punto {\red$C$} \`{e} detto \textbf{centro di istantanea
  rotazione} ed \`{e} il riferimento per la posizione globale del robot
\item I parametri di geometria per il modello cinematico sono:
  \begin{itemize}
    %%
  \item\red\footnotesize{}$r_L$, raggio della ruota sinistra
  \item\red$r_R$, raggio della ruota destra
  \item\red$n_L$, numero di tick per giro dell'encoder sinistro
  \item\red$n_R$, numero di tick per giro dell'encoder destro
  \item\red$L$, distanza tra i punti di contatto delle due ruote
    (\textbf{wheelbase})
    %%
  \end{itemize}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Parametri:
  \begin{itemize}
    %%
  \item\footnotesize{}$r_L$, raggio della ruota sinistra
  \item$r_R$, raggio della ruota destra
  \item$n_L$, numero di tick per giro dell'encoder sinistro
  \item$n_R$, numero di tick per giro dell'encoder destro
  \item$L$, distanza tra i punti di contatto delle due ruote
    (\textbf{wheelbase})
    %%
  \end{itemize}
%%
\item Detti $\Delta tick_L$ e $\Delta tick_R$ il numero di \textbf{tick
  contati}, su ruota destra e sinistra, nell'intervallo di campionamento
  $\Delta T$, abbiamo:
{\red{}
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta \theta = \frac{\Delta d_R - \Delta d_L}{L} & ~ & \Delta d = \frac{\Delta d_R + \Delta d_L}{2}\\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Detti $\Delta tick_L$ e $\Delta tick_R$ il numero di \textbf{tick
  contati}, su ruota destra e sinistra, nell'intervallo di campionamento
  $\Delta T$, abbiamo:
{\red{}
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta \theta = \frac{\Delta d_R - \Delta d_L}{L} & ~ & \Delta d = \frac{\Delta d_R + \Delta d_L}{2}\\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}}
%%
\item Dove:
  \begin{itemize}
    %%
  \item\footnotesize{}$\Delta d_L$, $\Delta d_R$, distanza percorsa (in
    $\Delta T$) dalla ruota sinistra/destra
  \item$\Delta theta$, variazione dell'orientamento (in radianti)
  \item$v_L, v_R$, velocit\`{a} istantanea della routa sinistra/destra
    %%
  \end{itemize}
\end{itemize}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{eqnarray*}
\Delta d_L  =  \frac{2 \pi r_L}{n_L}\Delta tick_L & ~ &
\Delta d_R  =  \frac{2 \pi r_R}{n_R}\Delta tick_R \\
\Delta \theta = \frac{\Delta d_R - \Delta d_L}{L} & ~ & \Delta d = \frac{\Delta d_R + \Delta d_L}{2}\\
v_L  =  \frac{\Delta d_L}{\Delta T} & ~ &
v_R  = \frac{\Delta d_R}{\Delta T}
\end{eqnarray*}
\begin{itemize}
%%
\item\footnotesize{}Sulla base delle relazioni precedenti, l'aggiornamento
  della posa si ottiene con:
{\red{}
\begin{eqnarray*}
 x  & = & x + \Delta d \cos(\theta + \frac{\Delta \theta}{2}) \\
y  & = &y + \Delta d \sin(\theta + \frac{\Delta \theta}{2}) \\
\theta & =& \theta + \Delta \theta
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
\begin{itemize}
%%
\item\footnotesize{}Sulla base delle relazioni precedenti, l'aggiornamento
  della posa si ottiene con:
{\red{}
\begin{eqnarray*}
 x  & = & x + \Delta d \cos(\theta + \frac{\Delta \theta}{2}) \\
y  & = &y + \Delta d \sin(\theta + \frac{\Delta \theta}{2}) \\
\theta & =& \theta + \Delta \theta
\end{eqnarray*}}
%%
\item Le relazioni precedenti, note come \textbf{formule per l'odometria},
  si determinano tramite l'\textbf{\red{}integrazione dell'equazione del
    moto}
%%
\item Tale integrazione (che non pu\`{o} essere effettuata analiticamente)
  \`{e} effettuata \textbf{numericamente} e pertanto contiene un
  \textbf{errore che si propaga {\red{}in modo additivo}} nella stima della
  posa $(x,y,\theta)$
%%
\item La posa stimata $(x,y,\theta)$ \`{e} pertanto soggetta ad un errore
  non nullo e non eliminabile che cresce con l'andare del tempo/distanza
  percorsa
%%
\end{itemize}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Cinematica del Robot Mobile a due ruote indipendenti}
%%
%%
\begin{center}
\includegraphics[scale=0.3]{robot_model_speed.eps}
\end{center}
%%
\begin{itemize}
%%
\item\footnotesize{}La \textbf{velocit\`{a}} \`{e} invece rappresentata
  secondo due possibili sistemi di riferimento
  \begin{itemize}
    %%
  \item\footnotesize{}$(v_L, v_R)$, velocit\`{a} istantanea delle
    routa sinistra e destra
  \item$(v, \omega)$, velocit\`{a} istantanea di \textbf{traslazione} e
    \textbf{rotazione} del punto $C$
  \end{itemize}
%%
\item Tra i due sistemi di riferimento esiste la seguente trasformazione:
%%
{\red\begin{eqnarray*}
v = \frac{v_R+v_L}{2}& ~ & \omega = \frac{v_R-v_L}{L}\\
v_L = v - \frac{L\omega}{2} & ~ & v_R = v + \frac{L\omega}{2}
\end{eqnarray*}}
%%
\end{itemize}
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Classe Kinematics}
%%
\begin{itemize}
%%
\item\footnotesize{}Nel sistema di Motion Control, la classe
\texttt{\textbf{\red{}Kinematics}} gestisce la cinematica del robot secondo
il modello illustrato
\item E' un \textbf{task periodico} che viene eseguito ogni $5ms$ (metodo
  \texttt{run()}), e comprende la lettura degli encoder ed il calcolo delle
  grandezze cinematiche
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
class Kinematics : public PeriodicTask {
 public:
    Kinematics(Pose & p,
               float radius_left, float radius_right,
               float wheel_base, float ticks);
    void run();
    float speed_left() { return m_speed_left; };
    float speed_right() { return m_speed_right; };
    Pose & pose() { return robot_pose;};
 private:
    float m_radius_left, m_radius_right;            // raggio ruote
    float m_wheel_factor_left, m_wheel_factor_right;// fattore tick/distanza
    float m_wheelbase;                              // distanza tra le ruote
    float m_ticks_per_revolution;                   // tick/giro encoder
    int m_poscount_left, m_poscount_right;          // distanza percorsa in DT
    Pose & robot_pose;                              // riferimento alla posa del robot
    float m_speed_left, m_speed_right;              // vel sx e dx istantanee
    float m_linear_speed, m_angular_speed;          // vel lineare e angolare
    float m_linear_distance, m_angular_distance;    // distanza lineare e angolare
};
\end{lstlisting}
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in velocit\`{a}, classe SpeedControlTask}
%%
\begin{itemize}
%%
\item\footnotesize{}La classe \texttt{\textbf{\red{}SpeedControlTask}}
  implementa i PID per il controllo di velocit\`{a} delle ruote sinistra e
  destra
%%
\item E' un \textbf{task periodico} che viene eseguito ogni $10ms$ (metodo
  \texttt{run()})
%%
\item Essa include un riferimento alla classe \texttt{\textbf{Kinematics}}
  in modo da poter conoscere le velocit\`{a} effettive delle due ruote
%%
\end{itemize}
%%
\black\clangtiny%%
\begin{lstlisting}
class SpeedControlTask : public PeriodicTask {
 public:
    SpeedControlTask(Kinematics & kin);
    void run();
    void off(bool stop_pwm = true);

    // imposta i parametri dei PID
    void set_params(float kp, float ki, float kd);

    // imposta le vel target
    void set_targets(float l, float r);

    // restituisce le vel correnti
    void get_current_speeds(float & l, float & r);
 private:
    Kinematics & m_kinematics;           // riferimento alla cinematica
    int m_pwm_left, m_pwm_right;         // valori del PWM impostati dai PID
    float m_target_left, m_target_right; // vel target
    Pid m_left_pid, m_right_pid;         // PID di ruota sx e dx
};
\end{lstlisting}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Relazioni tra Schema di Controllo e Oggetti Software}
%%
\includegraphics[scale=0.22]{position_control_software.eps}
%%
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in Posizione: Schema delle classi}
%%
\begin{itemize}
%%
\item\footnotesize{}La classe astratta
  \texttt{\textbf{\red{}PositionControl}} rappresenta il task periodico che
  implementa il \textbf{generico controllo} in posizione
%%
\item Essa include un riferimento all'oggetto \texttt{\textbf{Kinematics}}
  ed all'oggetto \texttt{\textbf{SpeedControlTask}}
%%
\item Da \texttt{\textbf{PositionControl}} vengono derivate le classi che
  implementano lo \textbf{specifico controllo}
%%
\end{itemize}
%%
\includegraphics[scale=0.22]{position_control_classes.eps}
%%
\end{frame}
%%




%%
\begin{frame}[fragile]
%%
  \frametitle{Relazioni tra Schema di Controllo e Oggetti Software}
%%
\includegraphics[scale=0.22]{position_control_software_2.eps}
%%
\end{frame}
%%



%%
\begin{frame}[fragile]
%%
  \frametitle{Controllo in Posizione ``Intercambiabile''}
%%
\includegraphics[scale=0.2]{position_control_classes.eps}
%%
\begin{itemize}
%%
\item\footnotesize{}Dei vari ``\texttt{PeriodicTask}'' che implementano gli
  specifici controlli in posizione, \textbf{solo uno per volta} pu\`{o}
  essere attivo
%%
\item Ci\`{o} viene realizzato andando ad ``accendere''/''spegnere'' i
  singoli task periodici
%%
\item Ogni task periodico pu\`{o} essere infatti acceso o spento con:
%%
  \begin{itemize}
    %%
  \item\footnotesize{}\textbf{metodo \texttt{\red{}void on()}}, accende il task
    %%
  \item\footnotesize{}\textbf{metodo \texttt{\red{}void off()}}, spegne il
    task (esso non verr\`{a} pi\`{u} schedulato)
    %%
  \end{itemize}
\end{itemize}
\end{frame}
%%





%%
\begin{frame}[fragile]
%%
  \frametitle{Path Control}
%%
\includegraphics[scale=0.2]{position_control_classes_2.eps}
%%
\begin{itemize}
%%
\item\footnotesize{}La classe \textbf{\texttt{\red{}PathControl}} gestisce
  una \textbf{lista di comandi} di geometria
%%
\item Usa un puntatore a \textbf{PositionControl} che rappresenta il
  controllore in posizione corrente
%%
\item Per cambiare controllo:
  \begin{itemize}
    %%
    \item\scriptsize{}\texttt{m\_current\_control->off();}
    \item\scriptsize{}\texttt{m\_current\_control = // nuovo controllo;}
    \item\scriptsize{}\texttt{m\_current\_control->set\_target(...);}
    \item\scriptsize{}\texttt{m\_current\_control->on();}
  \end{itemize}
%%
\end{itemize}
\end{frame}
%%





%%
\frame{\titlepage}
%%




%%
%%
\end{document}
%%
%%