%倒立擺-狀態反饋；小車一階
clear;
M=1.096;  %小車質量
m=0.109;%擺桿質量 
b=0.1; % 小車磨擦系數
l=0.25; %擺桿轉動軸心到桿質心的長度
I=0.0034; %擺桿慣量
f=0; %擺桿所受的磨擦阻力矩系數
g=9.8; %重力

p=I*(M+m)+M*m*l^2;
A=[0 1 0 0;
   0 -(I+m*l^2)*b/p  (m^2*g*l^2)/p  -m*l*f/p;
   0 0 0 1;
   0 -(m*l*b)/p  m*g*l*(M+m)/p  -(M+m)*f/p;];
B=[0;-(I+m*l^2)/p;0;-m*l/p];
C=[1 0 0 0;0 0 1 0];
D=[0;0];

disp('The rank of controllability mjatrix')
rc=rank(ctrb(A,B))
%p1=[-2-3*i,-2+3*i,-20,-21];
p1=[-20,-21;-2-3*i,-2+3*i];
k=place(A,B,p1)
Cn=[1 0 0 0];

s=size(A,1)
Z=[zeros([1,s]) 1]
N=inv([A,B;Cn,0])*Z'
Nx=N(1:s)
Nu=N(1+s)
Nbar=Nu+k*Nx

t0=0;
tspan=0.006;
R=0;
y0=[0; 0; 0.1; 0]

n=1000;
F=R*Nbar-k*y0
for i=1:n
    tf=t0+tspan;
    [t,y]=ode45(@dlb_k_fun,[t0,tf],y0,[],F);
    t0=tf;
    y1=y(end,1);
    y2=y(end,2);
    y3=y(end,3);
    y4=y(end,4);
    y0=[y1; y2; y3; y4];
    F=R*Nbar;
    F=F-k*y0;
    h(i,1)=y1;
    h(i,2)=y2;
    h(i,3)=y3;
    h(i,4)=y4;
    h(i,5)=F;
end

T=0.006:0.006:6;
figure(1);
plot(T,h(:,1),':b')
legend('小車位移')
grid
figure(2);
plot(T,h(:,3),'-r')
legend('擺桿擺角')
grid
figure(3);
plot(T,h(:,5),'--g')
legend('F')
grid