clear all
m=1.;
J0=1;
k=10;
c=7;
a=-.02;
V=3;
z0=.0;
dz0=0.;
alpha0=0.09;
dalpha0=1;
beta0=.1;
ro=1.2;
S=1.;
L=1.;
a0=1;
NPT=6000;
T=3;
dt=T/NPT;
temp=linspace(1,T+1,NPT)-1;
%=========
M=[m, a*cos(alpha0);a*cos(alpha0),J0];
K=[k ,-ro*S*V^2*dcz0(alpha0)/2;0., c-ro*S*V^2*L*dcm0(alpha0)/2];
R=linsolve(M,K);
Rad=linsolve(M,K');
%
F0=[ro*S*V^2*cz0(alpha0)/2,ro*S*L*V^2*cm0(alpha0)/2];
B0=[ro*S*V^2*dczg(beta0)/2,ro*S*L*V^2*dcmg(beta0)/2];
xeq=linsolve(K,F0');
%B0=[0,1];
F=linsolve(M,F0');
B=linsolve(M,B0');
beta=linspace(1,2,NPT)*0;
%==============
% critere de controlabilite
%==========
%==========
condinit= M*[z0;alpha0];
vitesseinit=M*[dz0;dalpha0]; 
xeqM=M*xeq;
z0M=condinit(1);
alpha0M=condinit(2);
dz0M=vitesseinit(1);
dalpha0M=vitesseinit(2);
%===============
PHIHUM=calculgramraideur(NPT,dt,z0M,dz0M,alpha0M,dalpha0M,xeqM,Rad,B0,F0,a0);
[P1,P2]=calculetatadj(NPT,dt,PHIHUM,Rad);
%===============
%===============
%===============
        for kk=1:NPT
     beta(kk)=-(B0(1)*P1(kk)+B0(2)*P2(kk))/a0;
        end
%=======
figure
subplot(1,3,1)
plot(temp,beta,'LineWidth',2)
xlabel ('temps')
ylabel(' Contrôle de raideur')
title('Loi de contrôle portant uniquement sur la raideur')
%===========
%=============
% resolution du systeme edo
%=============
z(1)=z0;
z(2)=z0(1)+dt*dz0(1);
alpha(1)=alpha0;
alpha(2)=alpha(1)+dt*dalpha0;
X1=[z(1),alpha(1)]';
X2=[z(2),alpha(2)]';

%===========

for i=3:NPT
X3=2*X2-X1+(dt^2)*(F+B*beta(i-1)-R*X2);
z(i)=X3(1);
alpha(i)=X3(2);
X1=X2;
X2=X3;
end


%=======
subplot(1,3,2)
plot(temp,z,temp,alpha,'LineWidth',2)
xlabel ('temps')
ylabel(' En bleu z et en rouge alpha')
title('Mouvements avec le contrôle de raideur')
%pause
%=======
X1=[z(NPT-1),alpha(NPT-1)]';
X2=[z(NPT),alpha(NPT)]';
zp=linspace(1,2,2*NPT);
alphap=linspace(1,2,2*NPT);
tempo=linspace(1,2*T+1,2*NPT)-1;
for i=1:NPT
    zp(i)=z(i);
    alphap(i)=alpha(i);
end

for i=1:NPT
X3=2*X2-X1+(dt^2)*(F-R*X2);
zp(i+NPT)=X3(1);
alphap(i+NPT)=X3(2);
X1=X2;
X2=X3;
end
%=======
subplot(1,3,3)
plot(tempo,zp,tempo,alphap,'LineWidth',2)
xlabel ('temps')
ylabel(' En bleu z et en rouge alpha')
title('Mouvements post-contrôle portant sur la raideur')
%=======