%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Programme pid_suspension.m %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all; clear all

%% Calcul du modèle de la suspension magnétique

% Paramètres de la suspension
m=1.75;
r=24;
k1=1.9e-4;
k2=-6.4e-4;
L0=0.3714;
g=9.81;

% Point de fonctionnement considéré
Z0=1.5e-2;
I0=sqrt(m*g/k1)*(Z0+k2);
U0=r*I0;
V0=-1000*Z0;

% Calcul des paramètres associés à la linéarisation autour du point
% de fonctionnement choisi
df_dZ=2*k1*I0^2/m/(Z0+k2)^3;
df_dI=-2*k1*I0/m/(Z0+k2)^2;
dg_dZ_point=-2*k1*I0/(Z0+k2)^2;
dg_dI_point=2*k1/(Z0+k2)+L0;

% Changement d'échelle de temps
df_dZ=df_dZ/100;
df_dI=df_dI/100;
dg_dZ_point=10*dg_dZ_point;
dg_dI_point=10*dg_dI_point;

% Fonction de transfert de la suspension
% P(s)=b0/(s^3+a2s^2-a1s-a0)
B=-1000*df_dI;
A=conv([1 0 -df_dZ],[dg_dI_point r])+[0 0 df_dI*dg_dZ_point 0];
P=tf(B,A);
b0=B/A(1);
a2=A(2)/A(1);
a1=-A(3)/A(1);
a0=-A(4)/A(1);

%% Régulateur PID pour la suspension magnétique

% Choix du paramètre Re (Re<-1)
Re=input('Quel paramètre Re (choisir Re<-1)')

% Choix de la fréquence de coupure w0 et de la marge de phase MP
omega_0=input('quel omega_0?');
MP_deg=input('quel marge de phase (en degrés)?');
MP=MP_deg/180*pi;

% Calcul du gain et de la phase de P(s) à w=w0 rad/s
P_omega_0j=b0/((omega_0*j)^3+a2*(omega_0*j)^2-a1*(omega_0*j)-a0);
gain_P_omega_0j=abs(P_omega_0j);
phase_P_omega_0j=angle(P_omega_0j);
phase_P_omega_0j=phase_P_omega_0j-2*pi;  % Matlab donne le résultat avec un décalage de 2pi.

% Choix de tau_i
tau_i=input('Quel tau_i?')

% Calcul de phi_i qui en résulte et du régulateur PI de gain=1 à w=w0.
phi_i=pi/2-atan(tau_i*omega_0);
k_i=tau_i*omega_0/sqrt(1+tau_i^2*omega_0^2);

% Calcul de phi_d qui en résulte et du régulateur PDF de phase phi_d et de
% gain=1 à w=w0.
phi_d=phi_i-pi+MP-phase_P_omega_0j;
a=(1+sin(phi_d))/(1-sin(phi_d));
tau_d=1/(omega_0*sqrt(a));
k_d=1/sqrt(a);

% Calcul du gain de réajustement k_p
k_p=1/gain_P_omega_0j;

% Fonction test pour l'équation (1.25)
residu=Re+k_p*k_i*k_d*b0/(a0*tau_i)*(tau_i+(a-1)*tau_d-a1/a0) 

% Test pour 0<phi_d<pi/2
if phi_d>pi/2
    display('phi_d incohérent --> faire de nouveaux choix')
    pause
else
end

% Calcul des régulateurs PI et PDF
s=tf('s');
K_PI=k_i*(1+1/(tau_i*s));
K_PDF=k_d*(1+a*tau_d*s)/(1+tau_d*s);

%% Lieux de Bode et de Nyquist
K_PID=k_p*K_PI*K_PDF;
sysBO=P*K_PID;
figure
bode(sysBO,K_PID,'-.',P,'--')
grid
figure
nyquist(sysBO)