%(能在Matlab6.5上運行)
clc;
clear all;
%初試化變量
E=5.97e7;%梁的彈性模量
Ep=1.895e7;%壓電片的彈性模量
d31=4.47e-10;%壓電片壓電常數
lb=0.450;%梁的長度
rho=2.5e3;%梁的密度
b=2.5e-2;%梁的寬度
h=1e-3;%梁的高度的一半(2h=0.002)
A=2*b*h;%梁的截面積
l=0.150;%壓電片長度;
hp=3.8e-4;%壓電片高度
I=(b*h^3)/12;%慣性矩
betal=[1.875,4.694,7.855];%beta與l的乘積
beta=betal/lb;%beta的值
Cs=6e-8;%壓電片電容
L1=0.15;
L2=L1+l;
I=(2*b*h^3)/12;%慣性矩
K=b*d31*Ep*(h+hp);%比例系數
Kb=0.5*b*d31*Ep*(hp+h)/Cs;
gamma=[];
w=[];
%計算omega的值--梁的前三階固有頻率
omega=betal.^2*sqrt(E*I/(rho*A*lb^4));
%計算gama
gamma=(sin(betal)-sinh(betal))./(cos(betal)-cosh(betal));
%創建矩陣A1
A1=[zeros(3),eye(3);-diag(omega.^2),zeros(3)];
%創建矩陣B
syms x
c=[];
for i = 1:3
    y = inline(rho*A*(cos(beta(i)*x)-cosh(beta(i)*x)+gamma(i)*(sin(beta(i)*x)-sinh(beta(i)*x))).^2);
    c(i)=sqrt(1/quad(y,0,lb));
end
psiD11 = c.*beta.*(-sin(beta*L1)-sinh(beta*L1)+gamma.*(cos(beta*L1)-cosh(beta*L1)));
psiD22 = c.*beta.*(-sin(beta*L2)-sinh(beta*L2)+gamma.*(cos(beta*L2)-cosh(beta*L2)));
B=K*[zeros(3,1);(psiD11-psiD22).'];
%創建矩陣C
C=-Kb*[(psiD22-psiD11),zeros(1,3)];
%創建矩陣Q,R
rou = 1e7;
Q=rou*eye(size(A1));
R=1;
[K2,S,e] = lqr(A1,B,Q,R);
sys = ss(A1,B,C,0);
%系統初始化q(i)
q=[];
syms x;
F=1e-1;
omegaX=inline(-F*(x.^3-3*l*x.^2)/(6*E*I));%撓曲線方程
for i = 1:3
    psi=inline(c(i)*(cos(beta(i)*x)-cosh(beta(i)*x)+gamma(i)*(sin(beta(i)*x)-sinh(beta(i)*x))));
    y=inline(rho*A*omegaX(x).*psi(x));
    q(i)=quad(y,0,lb);
end
x0=[q.';0;0;0];
t=[0:0.001:100];
[y,t,x]= initial(sys,x0,t);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A2=A1-B*K2;
sys1= ss(A2,zeros(size(B)),C,0);
[y1,t,x1] = initial(sys1,x0,t);
plot(t,x(:,1),'k-.',t,x1(:,1),'k-','Linewidth',1.);
set(gcf,'color','white');
set(gca,'fontsize',15,'Linewidth',1.5);
xlabel('t(s)');
ylabel('w(m)');