clear;
close all;
s=tf('s');
M=0.5;      % kg
m=0.2;      % kg
b=0.1;      % Ns/m
L=0.3;      % m
g=9.81;     % m/s2
I=0.006;    % kg*m2

G=L*m*s/(((M+m)*s+b)*((I+m*L^2)*s^2-L*m*g)-m^2*L^2*s^3);

poliFTCA=pole(G); % NB: 1 polo reale positivo --> FTCA instabile
                  % (criterio di Bode non applicabile)
          
t=0:0.01:30;
figure('Name','Risposta del pendolo in C.A. a impulso');
impulse(G,t);
figure('Name','Risposta del pendolo in C.A. a gradino');
step(G,t);

Kp=20;       % N/rad
Kd=10;       % Ns/rad
Ki=8;        % N/(rad*s)

Gpid=Kp+Kd*s+Ki/s;

Gcd=Gpid*G;          % FT in catena diretta
G1=feedback(Gcd,1);
poliFTCC=pole(G1);   % poli con parte reale negativa tranne uno nullo,
zeriFTCC=zero(G1);   % ma è calcolato anche uno zero nullo che annulla 
                     % il polo nullo, quindi FTCC stabile, di tipo 0
                     % perchè il polo nullo del controllore annulla
                     % lo zero nullo del sistema; infatti l'errore a
                     % gradino non è nullo
                     
figure('Name','Risposta del pendolo in C.C. a impulso');
impulse(0.001*G1,t);  % impulso di riferimento di ampiezza 0.001 rad*s
figure('Name','Risposta del pendolo in C.C. a gradino');
step(0.02*G1,t);      % gradino di riferimento di ampiezza 0.02 rad


Gdist=feedback(G,Gpid);

figure('Name','Risposta in C.C. a disturbo impulsivo, con rif. nullo');
impulse(0.01*Gdist,t);  % impulso di disturbo di ampiezza 0.01 N*s