%%
%% esercitazioni.tex
%%
\documentclass[a4paper, 12pt]{article}
\usepackage{a4wide}
\usepackage{times}
\usepackage{graphics}
\usepackage{graphicx}
\usepackage{amsmath}
\usepackage{listings}

\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}}

\newcommand{\pythonsmall}{
\lstset{language=Python, basicstyle=\ttfamily\scriptsize,frame=trBL,
  frameround=fttt}}


\title{Esercitazioni di Programmazione Sistemi Robotici}
\author{Prof. Corrado Santoro}
\date{A.A. 2016-17}

\begin{document}

\maketitle

\section{Esercizio 1}
\subsection{Testo}

Sia dato un sistema dinamico caratterizzato dalla equazione differenziale:
\begin{eqnarray*}
\ddot{y} + 0.5 \dot{y} + 3 y & = & u
\end{eqnarray*}
%%
dove $u$ \`{e} l'ingresso e $y$ \`{e} l'uscita;
%%
\begin{itemize}
%%
\item Discutere la \textbf{stabilit\`{a}} del sistema;
\item Calcolare il \textbf{guadagno statico};
\item Implementare il sistema usando un tempo di campionamento di $10~ms$ e
  tracciare la risposta al gradino unitario.
\end{itemize}
%%


%%
\subsection{Soluzione}
%%
Per \textbf{determinare} la stabilit\`{a} occorre calcolare i modi del
sistema, pertanto lo trasformiamo nella forma:
%%
\begin{eqnarray*}
\dot{x} & = & Ax + Bu\\
y & = & Cx
\end{eqnarray*}
%%

Si ponga $x_1 = y$, $x_2 = \dot{y}$; otteniamo il seguente sistema:
%%
\begin{eqnarray*}
\dot{x_1} & = & x_2\\
\dot{x_2} & = & -3 x_1 - 0.5 x_2 + u\\
y & = & x_1
\end{eqnarray*}
%%
da cui:
%%
\begin{eqnarray*}
\begin{bmatrix}
\dot{x_1} \\ \dot{x_2}
\end{bmatrix}
& = &
\begin{bmatrix}
0 & 1  \\ -3 & -0.5
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2
\end{bmatrix}
+
\begin{bmatrix}
0 \\ 1
\end{bmatrix}
u\\
%%
%%
y & = &
\begin{bmatrix}
1 & 0
\end{bmatrix}
\begin{bmatrix}
x_1 \\ x_2
\end{bmatrix}
%%
\end{eqnarray*}
%%
%%


Le matrici sono:
%%
\begin{eqnarray*}
A = \begin{bmatrix}
0 & 1 \\
-3 & -0.5
\end{bmatrix}
&
B = \begin{bmatrix}
0 \\ 1
\end{bmatrix}
\\
C = \begin{bmatrix}
1 & 0
\end{bmatrix}
&
\end{eqnarray*}


Determiniamo gli autovalori della matrice $A$, i quali sono le soluzioni
dell'equazione caratteristica:
\begin{eqnarray*}
| \lambda I - A | & = & 0
\end{eqnarray*}
%%
Calcoliamo $[\lambda I - A]$:
\begin{eqnarray*}
[\lambda I - A] & = &
\begin{bmatrix}
\lambda & 0 \\ 0 & \lambda
\end{bmatrix}
-
\begin{bmatrix}
0 & 1  \\ -3 & -0.5
\end{bmatrix}
\\
& = &
\begin{bmatrix}
\lambda & -1 \\
3 & \lambda + 0.5
\end{bmatrix}
%%
\end{eqnarray*}
%%
Calcoliamo il determinante:
\begin{eqnarray*}
\lambda (\lambda + 0.5) + 3 & = & \lambda^2 + 0.5\lambda + 3
\end{eqnarray*}
%%
Gli autovalori sono le soluzioni dell'equazione:
\begin{eqnarray*}
\lambda^2 + 0.5\lambda + 3 & = & 0
\end{eqnarray*}
ossia:
\begin{eqnarray*}
\lambda & = & \frac{-0.5 \pm \sqrt{0.25 - 12}}{2}\\
        & = & -\frac{0.5}{2} \pm \frac{\sqrt{12 - 0.25}}{2}i
\end{eqnarray*}
%%
Le radici sono \textbf{complesse e coniugate}; la parte reale \`{e}
\textbf{negativa}, pertanto il sistema \`{e} \textbf{asintoticamente
  stabile.}
%%

\vspace{0.5cm}


%%
Per il calcolo del \textbf{guadagno statico}, occorre determinare la \textbf{funzione
  di trasferimento}  $G(s)$, la quale \`{e} data da:
\begin{eqnarray*}
G(s) & = & C(sI-A)^{-1}B
\end{eqnarray*}
Determiniamo, in primo luogo, $sI-A$:
%%
\begin{eqnarray*}
(s I - A) & = &
\begin{bmatrix}
s & 0 \\ 0 & s
\end{bmatrix}
-
\begin{bmatrix}
0 & 1  \\ -3 & -0.5
\end{bmatrix}
\\
& = &
\begin{bmatrix}
s & -1 \\
3 & s + 0.5
\end{bmatrix}
%%
\end{eqnarray*}
%%
%%
quindi $(sI-A)^{-1}$:
%%
\begin{eqnarray*}
(s I - A)^{-1} & = &
\frac{%%
\begin{bmatrix}
s + 0.5 & 1 \\ -3 & s
\end{bmatrix}}
{s^2 + 0.5s + 3}
%%
\end{eqnarray*}
%%
Calcoliamo la funzione di trasferimento completa:
%%
\begin{eqnarray*}
G(s) & = & C(sI-A)^{-1}B\\
& = &
\begin{bmatrix}
1 & 0
\end{bmatrix}
\frac{%%
\begin{bmatrix}
s + 0.5 & 1 \\ -3 & s
\end{bmatrix}}
{s^2 + 0.5s + 3}
\begin{bmatrix}
0 \\ 1
\end{bmatrix}
\\
%%
& = &
\frac{%%
\begin{bmatrix}
s + 0.5 & 1
\end{bmatrix}}
{s^2 + 0.5s + 3}
\begin{bmatrix}
0 \\ 1
\end{bmatrix}
\\
%%
& = &
\frac{1}{s^2 + 0.5s + 3}
\end{eqnarray*}
%%

\vspace{0.5cm}

%%
Il \textbf{guadagno statico} \`{e} il valore a regime della risposta al
gradino $U(s) = \frac{1}{s}$, pertanto, applicando il teorema del valore
finale, risulta:
%%
\begin{eqnarray*}
K & = & \lim_{s\rightarrow 0}{s \frac{1}{s} \frac{1}{s^2 + 0.5s + 3}} = \\
& = & \lim_{s\rightarrow 0}{\frac{1}{s^2 + 0.5s + 3}} = \\
& = & \frac{1}{3}
\end{eqnarray*}


\vspace{0.5cm}

%%
Per \textbf{implementare} il sistema occorre discretizzarlo, pertanto
calcoliamo le matrici $A'$ e $B'$ del sistema discretizzato:
\begin{eqnarray*}
A' & = & A\Delta T + I \\
B' & = & B\Delta T
\end{eqnarray*}


\begin{eqnarray*}
A' & = &
\begin{bmatrix}
0 & 1  \\ -3 & -0.5
\end{bmatrix}
\Delta T +
\begin{bmatrix}
1 & 0  \\ 0 & 1
\end{bmatrix}
= \\
& = &
\begin{bmatrix}
1 & \Delta T  \\ -3\Delta T & -0.5\Delta T + 1
\end{bmatrix}
%%
\end{eqnarray*}

\begin{eqnarray*}
B' & = &
\begin{bmatrix}
0 \\ \Delta T
\end{bmatrix}
\end{eqnarray*}

Il sistema discretizzato pu\`{o} pertanto essere espresso nel seguente
modo:
\begin{eqnarray*}
x_1(k+1) & = & x_1(k) + x_2(k) \Delta T\\
x_2(k+1) & = &-3\Delta T  x_1(k) + (-0.5\Delta T + 1)x_2(k) + \Delta T
u(k)\\
y(k) & = & x_1(k)
\end{eqnarray*}

Per l'implementazione possiamo utilizzare la classe \texttt{DynamicSystem}
sviluppata durante il corso e creare una nuova classe denominata
\texttt{G1} nella quale il metodo \texttt{evaluate} implementa il sistema
desiderato.
%%
Il sorgente dell'implementazione risulta pertanto essere:
%%
\clangsmall
\begin{lstlisting}
#include <fstream>
#include "dynamic_system.h"

class G1 : public DynamicSystem {
public:
    G1(float delta_t) : DynamicSystem(delta_t) { x1 = 0; x2 = 0; };
    float evaluate(float input);
private:
    float x1, x2;
};

float G1::evaluate(float input)
{
    float y, x1_tmp, x2_tmp;

    x1_tmp = x1 + x2 * m_delta_t;
    x2_tmp = -3 * m_delta_t * x1 + (-0.5 * m_delta_t + 1)*x2 + m_delta_t * input;
    y = x1;
    x1 = x1_tmp;
    x2 = x2_tmp;
    return y;
}

int main(int argc, char **argv)
{
    G1 g(0.01);
    // Delta T = 10 ms

    float input = 1; // gradino unitario

    std::ofstream output_file("data.txt");
    // output file dei dati

    // 40 secs di simulazione
    for (int i = 0; i < 40 / 0.01;i++) {
        float output = g.evaluate(input);
        output_file << (i*0.01) << " " << output << std::endl;
    }

    output_file.close();
}
\end{lstlisting}


%%
%%
%%

\section{Esercizio 2}
\subsection{Testo}
Dato il sistema con funzione di trasferimento $G(s) = \frac{s+1}{s^2 + 2s + 2}$:
%%
\begin{itemize}
\item discutere la \textbf{stabilit\`{a}};
\item implementare il sistema utilizzando un controllore proporzionale in
  retroazione (vedi figura) e determinare sperimentalmente il valore a
  regime per $K = 1.5$ e gradino unitario come ingresso.
%%
Utilizzare $5~ms$ come tempo di campionamento.
%%
\end{itemize}
%%
\begin{center}
\includegraphics[scale=0.5]{closed_loop_2.pdf}
\end{center}
%%


%%
\subsection{Soluzione}
%%
Gli autovalori sono le soluzioni dell'equazione:
\begin{eqnarray*}
s^2 + 2 s + 2 & = & 0
\end{eqnarray*}
ossia:
\begin{eqnarray*}
s & = & \frac{-4 \pm \sqrt{4 - 8}}{2}\\
        & = & -2  \pm \frac{\sqrt{-4}}{2} \\
        & = & -2  \pm i \\
\end{eqnarray*}
%%
Le radici sono \textbf{complesse e coniugate}; la parte reale \`{e}
\textbf{negativa}, pertanto il sistema \`{e} \textbf{asintoticamente
  stabile.}
%%

\vspace{0.5cm}

Per l'implementazione occorre discretizzare il sistema, pertanto occorre
determinare la sua forma nello spazio di stato.
%%
Applicando la rappresentazione canonica abbiamo:
%%
\begin{eqnarray*}
A = \begin{bmatrix}
0 & 1 \\
-2 & -2
\end{bmatrix}
&
B = \begin{bmatrix}
0 \\ 1
\end{bmatrix}
\\
C = \begin{bmatrix}
1 & 1
\end{bmatrix}
&
\end{eqnarray*}


Per \textbf{implementare} il sistema occorre discretizzarlo, pertanto
calcoliamo le matrici $A'$ e $B'$ del sistema discretizzato:
\begin{eqnarray*}
A' & = & A\Delta T + I \\
B' & = & B\Delta T
\end{eqnarray*}


\begin{eqnarray*}
A' & = &
\begin{bmatrix}
0 & 1  \\ -2 & -2
\end{bmatrix}
\Delta T +
\begin{bmatrix}
1 & 0  \\ 0 & 1
\end{bmatrix}
= \\
& = &
\begin{bmatrix}
1 & \Delta T  \\ -2\Delta T & -2\Delta T + 1
\end{bmatrix}
%%
\end{eqnarray*}

\begin{eqnarray*}
B' & = &
\begin{bmatrix}
0 \\ \Delta T
\end{bmatrix}
\end{eqnarray*}


Il sistema discretizzato pu\`{o} pertanto essere espresso nel seguente
modo:
\begin{eqnarray*}
x_1(k+1) & = & x_1(k) + x_2(k) \Delta T\\
x_2(k+1) & = &-2\Delta T  x_1(k) + (-2\Delta T + 1)x_2(k) + \Delta T
u(k)\\
y(k) & = & x_1(k) + x_2(k)
\end{eqnarray*}



Per l'implementazione possiamo utilizzare la classe \texttt{DynamicSystem}
sviluppata durante il corso e creare una nuova classe denominata
\texttt{G} nella quale il metodo \texttt{evaluate} implementa il sistema
desiderato.
%%
Il sorgente dell'implementazione del sistema risulta pertanto essere:
%%
\clangsmall
\begin{lstlisting}
#include <fstream>
#include "dynamic_system.h"

class G : public DynamicSystem {
public:
    G(float delta_t) : DynamicSystem(delta_t) { x1 = 0; x2 = 0; };
    float evaluate(float input);
private:
    float x1, x2;
};

float G::evaluate(float input)
{
    float y, x1_tmp, x2_tmp;

    x1_tmp = x1 + x2 * m_delta_t;
    x2_tmp = -2 * m_delta_t * x1 + (-2 * m_delta_t + 1)*x2 + m_delta_t * input;
    y = x1 + x2;
    x1 = x1_tmp;
    x2 = x2_tmp;
    return y;
}
\end{lstlisting}

\vspace{0.5cm}

A questo punto simuliamo, nel \texttt{main}, il sistema a ciclo chiuso
includendo il controllore:
%%
\clangsmall
\begin{lstlisting}
int main(int argc, char **argv)
{
    float delta_t = 0.005;
    // Delta T = 5 ms
    G g(delta_t);

    float input = 1; // gradino unitario
    float y = 0;

    std::ofstream output_file("data.txt");
    // output file dei dati

    float KP = 1.5;

    // 10 secs di simulazione
    for (int i = 0; i < 10 / delta_t;i++) {
        float err = input - y;
        float output_controller = err * KP;
        y = g.evaluate(output_controller);
        output_file << (i*delta_t) << " " << y << std::endl;
    }

    output_file.close();
}
\end{lstlisting}
%%
Dalla simulazione si evince che il valore a regime risulta essere $\simeq 0.43$.


%%
%%
%%
%%

\section{Esercizio 3}
\subsection{Testo}
Dato il sistema il figura con $G1(s) = \frac{1}{s + 5}$ e $G2(s) =
\frac{3}{s + 2}$, implementare il sistema complessivo (senza determinare la
funzione di trasferimento totale) determinando sperimentalmente:
%%
\begin{itemize}
\item il tempo di salita per $K = 2.5$;
\item il valore a regime per $K=2.5$  e ingresso pari al gradino unitario.
%%
Utilizzare $5~ms$ come tempo di campionamento.
%%
\end{itemize}
%%
\begin{center}
\includegraphics[scale=0.5]{closed_loop_3.pdf}
\end{center}
%%


%%
\subsection{Soluzione}
%%
Occorre discretizzare $G1(s)$ e $G2(s)$. Essendo due sistemi del primo
ordine possiamo passare facilmente al dominio del tempo.
%%

Per il primo sistema abbiamo:
%%
\begin{eqnarray*}
Y(s) = & = & \frac{1}{s+5}U(s) \\
sY(s) + 5Y(s) & = & U(s)\\
\dot{y} + 5 y & = & u\\
\dot{y} & = & - 5 y + u
\end{eqnarray*}

discretizzando:
\begin{eqnarray*}
\frac{y(k+1) - y(k)}{\Delta T} & = & -5 y(k) + u(k)\\
y(k+1) - y(k) & = & -5 y(k)\Delta T + u(k) \Delta T\\
y(k+1)  & = & (-5\Delta T + 1)  y(k) + u(k) \Delta T
\end{eqnarray*}


%%

Per il secondo sistema otteniamo:
%%
\begin{eqnarray*}
Y(s) = & = & \frac{3}{s+2}U(s) \\
sY(s) + 2Y(s) & = & 3 U(s)\\
\dot{y} + 2 y & = & 3 u\\
\dot{y} & = & - 2 y + 3 u
\end{eqnarray*}

discretizzando:
\begin{eqnarray*}
\frac{y(k+1) - y(k)}{\Delta T} & = & -2 y(k) + 3 u(k)\\
y(k+1) - y(k) & = & -2 y(k)\Delta T + 3 u(k) \Delta T\\
y(k+1)  & = & (-2\Delta T + 1)  y(k) + 3 u(k) \Delta T
\end{eqnarray*}


Implementando il sistema otteniamo:
%%
\clangsmall
\begin{lstlisting}
#include <fstream>
#include "dynamic_system.h"

class G1 : public DynamicSystem {
public:
    G1(float delta_t) : DynamicSystem(delta_t) { y = 0; };
    float evaluate(float input);
private:
    float y;
};

float G1::evaluate(float input)
{
    y =(-5 * m_delta_t + 1)*y + m_delta_t * input;
    return y;
}

class G2 : public DynamicSystem {
public:
    G2(float delta_t) : DynamicSystem(delta_t) { y = 0; };
    float evaluate(float input);
private:
    float y;
};

float G2::evaluate(float input)
{
    y =(-2 * m_delta_t + 1)*y + 3 * m_delta_t * input;
    return y;
}


int main(int argc, char **argv)
{
    float delta_t = 0.005;
    // Delta T = 5 ms
    G1 g1(delta_t);
    G2 g2(delta_t);

    float input = 1; // gradino unitario
    float y = 0;

    std::ofstream output_file("data.txt");
    // output file dei dati

    float KP = 2.5;

    // 10 secs di simulazione
    for (int i = 0; i < 5 / delta_t;i++) {
        float err = input - y;
        float output_controller = err * KP;
        y = g1.evaluate(output_controller) + g2.evaluate(output_controller);
        output_file << (i*delta_t) << " " << y << std::endl;
    }

    output_file.close();
}
\end{lstlisting}
%%

Dal grafico della risposta a gradino otteniamo i valori approssimati del
tempo di salita $T_s \simeq 0.5~s$ e valore a regime $\simeq 0.8$






\section{Esercizio 4}
\subsection{Testo}
Considerando il sistema dell'esercizio 3, utilizzare un controllore di tipo
proporzionale-integrale (PI) e determinare $KP$ e $KI$ in modo da avere:
%%
\begin{itemize}
\item tempo di salita pari almeno a $\simeq 0.25 s$;
\item tempo di assestamento pari almeno a $\simeq 0.5 s$;
\item errore a regime nullo;
\item assenza di sovraelongazione.
%%
\end{itemize}
%%
Utilizzare $5~ms$ come tempo di campionamento.
%%


%%
\subsection{Soluzione}
%%
Aggiungiamo al sorgente dell'esercizio precedente un controllore PI:
\clangsmall
\begin{lstlisting}
class PI : public DynamicSystem {
public:
    PI(float delta_t, float kp, float ki) :
        DynamicSystem(delta_t), m_kp(kp), m_ki(ki), m_integral(0) { };
    float evaluate(float input);
private:
    float m_kp;
    float m_ki;
    float m_integral;
};

float PI::evaluate(float input)
{
    m_integral = m_integral + input * m_delta_t;
    return input * m_kp + m_integral * m_ki;
}
\end{lstlisting}
%%

%%
\vspace{0.5cm}
%%
Modifichiamo dunque il \texttt{main} in modo da includere il controllore
PI:
%%
\clangsmall
\begin{lstlisting}
int main(int argc, char **argv)
{
    float delta_t = 0.005;
    // Delta T = 5 ms
    G1 g1(delta_t);
    G2 g2(delta_t);
    PI controller(delta_t, 5, 10);

    float input = 1; // gradino unitario
    float y = 0;

    std::ofstream output_file("data.txt");
    // output file dei dati

    // 10 secs di simulazione
    for (int i = 0; i < 5 / delta_t;i++) {
        float err = input - y;
        float output_controller = controller.evaluate(err);
        y = g1.evaluate(output_controller) + g2.evaluate(output_controller);
        output_file << (i*delta_t) << " " << y << std::endl;
    }

    output_file.close();
}
\end{lstlisting}
%%
\vspace{0.5cm}
%%
Effettuando il tuning dei parametri si nota che per $KP = 5$ e $KI = 10$ si
ottengono le prestazioni specificate nella consegna.
%%


\end{document}