121 lines
2 KiB
Mathematica
121 lines
2 KiB
Mathematica
|
% Ce script permet de d<EFBFBD>clarer les valeurs des param<EFBFBD>tres du mod<EFBFBD>le
|
|||
|
|
% Il sert aussi <EFBFBD> d<EFBFBD>clarer et <EFBFBD> calculer les diff<EFBFBD>rentes variables
|
|||
|
|
% de commande tout au long du TP (Exemples : Point de fonctionnement,
|
|||
|
|
% trajectoire, commande <EFBFBD> grand gain, ...
|
|||
|
|
clear, clc,
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
%% Param<EFBFBD>tres :
|
|||
|
|
m = 500; %kg
|
|||
|
|
Mc = 5000; %kg
|
|||
|
|
g = 10; %m/s^2
|
|||
|
|
J = 50; %kg.m^2
|
|||
|
|
b = 0.4; %m
|
|||
|
|
Cd = 20; %kg/s
|
|||
|
|
Cr = 20; %kg.m^2/s
|
|||
|
|
|
|||
|
|
%% Point de fonctionnement
|
|||
|
|
R = 10;
|
|||
|
|
D = 0;
|
|||
|
|
C0 = m*g*b;
|
|||
|
|
F = 0;
|
|||
|
|
|
|||
|
|
%% Trajectoire
|
|||
|
|
Rini = R;
|
|||
|
|
Dini = 0;
|
|||
|
|
Rfin = 5;
|
|||
|
|
Dfin = 20;
|
|||
|
|
zh = 1;
|
|||
|
|
dt = 10;
|
|||
|
|
%% Manip 1
|
|||
|
|
A = [0 0 0 1 0 0 ;
|
|||
|
|
0 0 0 0 1 0 ;
|
|||
|
|
0 0 0 0 0 1 ;
|
|||
|
|
0 0 m*g/Mc -Cd/Mc 0 0 ;
|
|||
|
|
0 0 0 0 -Cr/(b^2*(J/b^2+m)) 0 ;
|
|||
|
|
0 0 -g/R*(1+m/Mc) Cd/(Mc*R) 0 0];
|
|||
|
|
|
|||
|
|
B = [0 0 ;
|
|||
|
|
0 0 ;
|
|||
|
|
0 0 ;
|
|||
|
|
1/Mc 0 ;
|
|||
|
|
0 -1/(b*(J/b^2+m)) ;
|
|||
|
|
-1/(R*Mc) 0 ];
|
|||
|
|
C= eye(6);
|
|||
|
|
Com = [B A*B A^2*B A^3*B A^4*B A^5*B];
|
|||
|
|
rank(Com)
|
|||
|
|
|
|||
|
|
%% Manip 2
|
|||
|
|
|
|||
|
|
vprA = damp(eig(A)); % valeur propres de A
|
|||
|
|
omega0 = vprA(2);
|
|||
|
|
xi = 0.5
|
|||
|
|
i=complex(0,1);
|
|||
|
|
p1 = omega0*(-xi+ i *sqrt(1-xi^2));
|
|||
|
|
p2 = omega0*(-xi- i *sqrt(1-xi^2));
|
|||
|
|
p = [-2 -2.5 -3 -4 p1 p2];
|
|||
|
|
|
|||
|
|
|
|||
|
|
K = place(A,B,p)
|
|||
|
|
C_1 =C(1,:);
|
|||
|
|
B_1 =B(:,1);
|
|||
|
|
|
|||
|
|
eta = -1/(C_1*(A-B*K)^-1*B_1);
|
|||
|
|
|
|||
|
|
%%
|
|||
|
|
sys = ss(A-B*K,B(:,2),C(2,:),D)
|
|||
|
|
H = tf(sys)
|
|||
|
|
bode(H)
|
|||
|
|
% margin(H)
|
|||
|
|
% Avec le correcteur intégral :
|
|||
|
|
Ti = 5e-4
|
|||
|
|
CI = tf(1,[-Ti 0]); % négatif pour avoir une phase >180
|
|||
|
|
%bode(H,CI*H,'grid')
|
|||
|
|
fig =figure();
|
|||
|
|
margin(CI*H)
|
|||
|
|
grid on;
|
|||
|
|
saveas(fig,"manip_5marge.png");
|
|||
|
|
|
|||
|
|
%% pretty figure
|
|||
|
|
% plot(simout)
|
|||
|
|
% legend("d","r","\theta","d d/dt","dr/dt","d\theta/dt");
|
|||
|
|
% grid on;
|
|||
|
|
%% Commande grand gain
|
|||
|
|
Deltat= 10
|
|||
|
|
Dini = 0;
|
|||
|
|
Rini = 10;
|
|||
|
|
Dfin = 20;
|
|||
|
|
Rfin = 5;
|
|||
|
|
ed = omega0/10;
|
|||
|
|
er = ed;
|
|||
|
|
|
|||
|
|
coeff_phi = [6/Deltat^5 -15/Deltat^4 10/Deltat^3 0 0 0]
|
|||
|
|
|
|||
|
|
|
|||
|
|
%%
|
|||
|
|
close all;
|
|||
|
|
fig= figure()
|
|||
|
|
plot(Dc)
|
|||
|
|
hold on
|
|||
|
|
plot(simout.Time, simout.Data(:,1));
|
|||
|
|
xlabel('temps (s)');
|
|||
|
|
grid on;
|
|||
|
|
title('Vérification de la planification de trajectoire');
|
|||
|
|
legend('d_c','d');
|
|||
|
|
|
|||
|
|
fig2= figure()
|
|||
|
|
plot(Rc)
|
|||
|
|
hold on
|
|||
|
|
plot(simout.Time, simout.Data(:,2));
|
|||
|
|
xlabel('temps (s)');
|
|||
|
|
grid on;
|
|||
|
|
title('Vérification de la planification de trajectoire');
|
|||
|
|
legend('r_c','r');
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|