%% CONTROLLI AUTOMATICI
% 
% ATTENZIONE: LE PRIME 2-3 ESERCITAZIONI SARANNO ESEGUITE PASSO PASSO DAGLI
% ESERCITATORI, LE SUCCESSIVE LE DOVRANNO ESSERE SVOLTE IN MODO AUTONOMO
% Tutte le esercitazioni sono pensate per capire meglio e applicare la teoria.
% ======================================================================
% Esercitazione Matlab n. 1: Matlab e Sistema Massa-Molla
% ======================================================================
% Matlab è un interprete di comandi. Il programma è così strutturato: 
% - command window: dove si possono digitare i comandi
% - workspace: dove tutte le variabili attualmente esistenti e disponibili 
%              sono visualizzate
% - command history: dove compare la lista dei comandi eseguiti nella command window
% - notare che in basso a sinistra è visualizzato lo "stato" in cui si trova 
%   matlab, per poter lavorare deve esserci scritto "Ready"
% I comandi si possono digitare dal prompt oppure possono essere editati 
% in un file con estensione .m che viene creato usando l'editor di matlab.
% Tutte le istruzioni inserite nel file .m vengono interpretate ed eseguite 
% in sequenza digitando il nome del file dal prompt (senza estensione .m).
% Matlab è composto da una serie di toolbox che importano funzioni
% specifiche che dipendono dal tipo di toolbox.
% NOTE:
% 1- Non assegnare mai ad un file o ad una variabile un nome che inizi con 
%    carattere numerico
% 2- Il simbolo "%" indica i commenti
% 3- Matlab è case-sensitive


% L'istruzione per pulire il workspace cancellando tutte le variabili in
% memoria è la seguente.
clear all


% 1 - SISTEMA DINAMICO RAPPRESENTATO COME "STATE-SPACE"
% ======================================================================
% Inseriamo (definiamo) le variabili (il ";" a fine riga impedisce la
% visualizzazione sulla command window). 
% In questo caso si tratta dei parametri del sistema in esame.
M = 1.0;        % kg  
k = 0.25;       % N/m
h = 0.5;        % N*s/m 
% h = 0;        % N*s/m 
% h = 1.5;      % N*s/m 
% h = 0.1;      % N*s/m 
% PROVA: togliere un ; per vedere la visualizzazione sulla command window


% Inseriamo poi le matrici del sistema dinamico in esame: le matrici sono 
% definite fra [], ogni sequenza di valori separata da un ";" rappresenta 
% una riga della matrice, mentre le diverse colonne sono separate da uno spazio.
% Nota bene: B è un vettore colonna, mentre C è un vettore riga.
% Importante notare le dimensioni delle matrici che sono concordi con le
% dimensioni delle variabili di stato, di ingresso e di uscita.
% x(1): è la posizione del carrellino.
% x(2): è la velocità  del carrellino.
A = [0 1; -k/M -h/M];
B = [0; 1/M];
C = [1 0];
D = 0;


% NOTE SULLE MATRICI e FUNZIONI:
% 1- costante*Matrice: moltiplicare ogni elemento di Matrice per una cost.
% 2- eye(n): matrice identità .
% 3- zeros(n) oppure zeros(m,n): matrice nxn o mxn di tutti zeri.
% 4- ones(n) oppure ones(m,n): matrice nxn o mxn di tutti uni.
% 5- diag(vettore): restituisce una matrice diagonale con elementi sulla
%    diagonale uguali a quelli del vettore.
% 6- diag(Matrice): restituisce la diagonale di una matrice.
% 7- Matrice1 * Matrice2: è il prodotto matriciale.
% 8- Matrice1 .* Matrice2: prodotto elemento per elemento.
% 9- Matrice' oppure vettore': è trasposta della matrice o del vettore.


% DEFINIZIONE DEL SISTEMA IN VARIABILI DI STATO
% Definiamo poi il sistema dinamico studiato come una "variabile" qui 
% denominata "sys". "ss" è una funzione Matlab ("state space"). Per una 
% completa descrizione dell'utilizzo di ciascuna funzione si digiti "help 
% nomedellafunzione". "help" da solo restituisce un elenco di tutti 
% i "toolbox" (insiemi di istruzioni e funzioni) disponibili -> "help 
% nometoolbox" offre un elenco delle istruzioni e funzioni del singolo 
% toolbox.
sys = ss(A,B,C,D);
% Se le dimensioni non sono coerenti tra loro la funzione "ss" restituisce
% un errore segnalando quali matrici non sono coerenti.
% Attenzione: i nomi da dare alle matrici non sono vincolanti, mentre la 
% posizione lo è. Es se chiamiamo la matrice C "pippo" (ovvero pippo=[1 0])
% basterà usarla opportunamente: ss(A,B,pippo,D)


% PROVA ESECUZIONE
% Possiamo eseguire questo file (che è solo un elenco di comandi!) o
% scrivendo il nome del file nel "command window", premendo "F5" oppure 
% l'icona di esecuzione nell'editor di testo del file.
% Tutte le variabili dichiarate sono all'interno di un workspace. Ogni
% variabile è richiamata con il suo nome.
% Scrivere nella command window sys e notare il suo contenuto come sistema
% lineare.


% 2 + 4 - MOVIMENTO LIBERO
% ======================================================================
% Visualizziamo graficamente l'andamento dell'uscita del sistema
% conseguente ad una perturbazione dello stato ("movimento libero").
% Il sistema massa - molla è all'equilibrio in x = [0 0], u = 0.
% In questo esempio si pone x1 = 1, x2 = 0 (è come se il carrello
% venisse spostato in x1 = 1, e poi rilasciato con velocità iniziale
% nulla).
x0 = [1; 0];
% Apriamo una figura su cui successivamente plottare.
figure(1)
initial(sys,x0)
% Come specificato dall'help ("help initial"), è possibile far calcolare 
% alla funzione i valori dell'andamento di stato e uscita, anzichè 
% tracciarli graficamente. Questo si può fare successivamente
% con la funzione "plot".
% Nota been: la initial simula la prima parte dell'eq di Lagrange che
% è stata vista a lezione. 
% PROVA: cambiare x0
% PROVA: cambiare C e far plottare x2
pause  % stoppa l'esecuzione dello script finchè un qualsiasi tasto viene premuto 
       % Attenzione: nell'angolo in basso a sinistra nella finestra 
       % principale di matlab viene segnalato lo stato in cui si trova:
       %    - "Busy": matlab sta lavorando, attendere
       %    - "Paused": matlab è in pausa, premere un tasto per continuare
close all  %chiude tutte le finestre aperte fino a questo momento

% PROVA 2.1: salvataggio degli andamenti di stato e uscite
% Salviamo gli andamenti di stato e uscita, più il vettore tempo,
% in Y,T,X (la sequenza degli output della funzione dipendono da come è 
% stata scritta la funzione initial).
[Y,T,X] = initial(sys,x0);
figure(1)
% Tracciamo ora i grafici:
% a- andamento dell'uscita (Y)
plot(T,Y,'r')
%       la funzione plot dati due vettori, plotta i punti dei due vettori x
%       e y e interpolando per ottenere un grafico.
grid on
%       plotta la griglia, bisogna utilizzarlo per ogni figura in cui si 
%       vuole la griglia.
% b- andamento dei due stati (X)
figure(2)
plot(T,X) 
grid on
pause
close all

% PROVA 2.2: specificare l'istante di tempo finale (tf).
tf = 50;
[Y,T,X] = initial(sys,x0,tf);
figure(1)
plot(T,Y,'r')
grid on 
figure(2)
plot(T,X) 
grid on
pause
close all

% PROVA 2.3: specificare tutto il vettore tempo equispaziato fra 0 e 40 sec,
%          e di 1000 elementi.
t = linspace(0,40,1000);
[Y,T,X] = initial(sys,x0,t);
% osservazione: in questo caso T coinciderà con t.
figure(1)
plot(T,Y,'r')
grid on
figure(2)
plot(T,X) 
% PROVA: controllare dal workspace la lunghezza dei vettori restituiti
%        che devono essere di 1000 elementi
grid on
pause
close all

% NOTE SUGLI ELEMENTI DI UNA MATRICE 
% Sia V una matrice di tot righe e colonne.
% V(i,j)   seleziona l'elemento sulla riga i e colonna j di V;
% V(:,j)   seleziona tutta la colonna j-esima;
% V(i,:)   seleziona tutta la riga i-esima;
% V(i,end) seleziona l'ultimo elemento della riga i;
% V(end,j) seleziona l'ultimo elemento della colonna j;
% V(:,end) seleziona tutta l'ultima colonna;
% V(end,:) seleziona tutta l'ultima riga.

% PROVA 2.4: Movimento dello stato nel tempo.
figure(1)
plot(T,X(:,1),'r')
grid on
hold on % il seguente grafico viene plottato senza sovrascrivere il precedente
plot(T,X(:,2),'g')
pause
close all




% 3 + 4 - RISPOSTA AD IMPULSO (MOVIMENTO FORZATO)
% ======================================================================
% Ripristino del sistema iniziale
impulse(sys)
pause
% Risposta ad impulso di ampiezza unitaria
[Y,T,X] = impulse(sys);
% impulse è una funzione che pone in ingresso al sistema sys un impulso.
figure(1)
plot(T,Y,'r')
grid on 
figure(2)
plot(T,X) 
grid on
pause
close all

% 5 + 4 - VARIARE SMORZAMENTO DEL SISTEMA
% ======================================================================
% h=0  : si provi ora a ripetere la stessa prova ponendo nei dati iniziali
%        del sistema h=0. Senza uno smorzamento il sistema massa-molla è un
%        sistema stabile, ma non asintoticamente.
% h>=1 : si noti che le oscillazioni scompaiono.
% h=0.1: si noti l'elevato numero di oscillazioni (ragionando fisicamente è
%        normale, nel senso che lo smorzamento dell'attrito è basso).

% Utilizzo di ciclo for per variare lo smorzamento del sistema.
% Ripetiamo ora quanto visto in precedenza in un ciclo for in cui
% facciamo variare il valore di h, assegnandogli di volta in volta
% un valore preso da un vettore H che definiamo
H = [0 0.25 0.5 0.75 1 1.5 2];
% Ad ogni esecuzione del ciclo, cambiando h andrà  ridefinita la
% matrice A ed il sistema sys
% length() restituisce la lunghezza del vettore passatogli in ingresso
figure(1)
for i = 1:length(H) % la notazione con i ':' , es a:b, crea un vettore che
                    % parte da 'a' ed arriva al valore 'b', con valori 
                    % intervallati ogni 1 
                    % ATTENZIONE: in Matlab gli indici partono da 1 e non da 0
    h = H(i);       % i assume ad ogni iterazione il valore di un elemento del vettore [1:length(H)]
    A = [0 1; -k/M -h/M];
    sys = ss(A,B,C,D);
    [Y,T,X] = initial(sys,x0,t);
    plot(T,Y),           grid on, hold on 
    pause
end

figure(2)
for h = H     
    A = [0 1; -k/M -h/M];
    sys = ss(A,B,C,D);
    [Y,T,X] = impulse(sys,t);
    plot(T,Y),           grid on, hold on 
    pause
end
close all


% RISPOSTA ALLO SCALINO
% ======================================================================
t = 0:0.02:20;      % se un vettore è definito con i doppi :, es a:c:b, 
                    % significa che parte da a, arriva al valore b ed ogni 
                    % elemento è equispaziato dal precedente di c (se
                    % omesso c=1)
[Y,T,X] = step(sys,t);
% Con la funzione step() riusciamo a dare in ingresso al sistema uno
% scalino. Ci aspettiamo che, se il sistema si stabilizza, la velocita' si
% annulli e il carrellino si trovi in una posizione diversa, perche'
% comunque abbiamo un ingresso costante sul carrellino.

figure(1)
% subplot(m,n,p) permette di creare delle sottofigure nella finestra. m
% indica il numero di righe delle sottofigure, n il numero delle colonne
% delle sottofigure e p l'i-esima sottofigura contata riga per riga da
% sinistra a destra.
subplot(1,2,1)
plot(T,X)
title('Risposta a scalino dello stato') %inserisce il titolo nella figura, in particolare nel subplot selezionato
grid on
legend('x1-posizione','x2-velocità') %inserisce la legenda, la prima stringa è associata al primo plor, la seconda al secondo e così via
subplot(1,2,2)
plot(T,Y)
title('Risposta a scalino dell''uscita') %n.b.= il doppio apice inserisce il carattere ' nella stringa
grid on


% TRENO DI IMPULSI
% ======================================================================
% Supponiamo di sollecitare il sistema con un treno di impulsi dati
% con frequenza preimpostata (ad. es uno al secondo). Inizialmente il
% sistema sara' all'equilibrio, con x = [0 0]. Se dopo un secondo dal
% primo impulso ricevuto lo stato si trovera' in un punto differente,
% conseguentemente al secondo impulso la sua evoluzione nel tempo sara'
% data dalla somma di due contributi (essendo il sistema lineare, vale
% il principio di sovrapposizione degli effetti):
% - il movimento libero (che si calcola con la initial);
% - il movimento forzato (che si calcola con la impulse);
% Si trattera' di calcolare di volta in volta gli andamenti di stato e
% uscita del sistema, concatenando i risultati in vettori Y, X e T.
deltaImpulsi=1;
ampiezzaImpulsi=2;
numeroImpulsi=40;
t = linspace(0,deltaImpulsi,20);
x = [0 0];
Y = [];
X = [];
T = [];
for i = 1:numeroImpulsi
    [yl,tl,xl] = initial(sys,x,t);
    [yf,tf,xf] = impulse(sys*ampiezzaImpulsi,t);
    Y = [Y; yl+yf];
    X = [X; xl+xf];
    T = [T t+deltaImpulsi*(i-1)];
    x = xf(end,:) + xl(end,:);
end
% N.B. t e' un vettore riga, yl, yf, xl ed xf sono vettori colonna.
% DOMANDA: nel codice c'e' un elemento ridondante che potrebbe essere
%          portato fuori dal ciclo for: quale? 
% RISPOSTA: "[yf,tf,xf] = impulse(sys,t);"
%           anche la teoria ci farebbe rispondere allo stesso modo, perche'
%           il movimento libero dipende dallo stato iniziale, mentre il
%           movimento forzato dipende dall'ingresso e non dallo stato
%           iniziale, quindi per sistemi lineari tempo invariante, dato un 
%           valore dell'input conosciamo a priori il valore del movimento
%           forzato.
figure(1)
subplot(2,1,1)
plot(T,Y), grid on
title('Risposta dell''uscita al treno degli impulsi')
subplot(2,1,2)
plot(T,X), grid on
title('Risposta degli stati al treno degli impulsi')
pause
close all




