# Matlab 作業（e94076152）:

## 作業1

clear all, close all, clc

m = 1;
M = 5;
L = 2;
g = -10;
d = 1;

s = 1; % pendulum up (s=1)

A = [0 1 0 0;
0 -d/M -m*g/M 0;
0 0 0 1;
0 -s*d/(M*L) -s*(m+M)*g/(M*L) 0];

B = [0; 1/M; 0; s*1/(M*L)];

Q = [1 0 0 0;%1 0 0 0
0 10 0 0;%0 1 0 0
0 0 10 0;
0 0 0 10];
R = .0001;

%%
det(ctrb(A,B))

%%
K = lqr(A,B,Q,R);

tspan = 0:.001:15;
if(s==-1)
y0 = [0; 0; 0; 0];
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[4; 0; 0; 0])),tspan,y0);
elseif(s==1)
y0 = [-3; 0; pi+.4; 0];
% [t,y] = ode45(@(t,y)((A-B*K)*(y-[0; 0; pi; 0])),tspan,y0);
[t,y] = ode45(@(t,y)cartpend(y,m,M,L,g,d,-K*(y-[1; 0; pi; 0])),tspan,y0);
else

end

for k=1:100:length(t)
drawcartpend_bw(y(k,:),m,M,L);
end

[eigvector,eigvalue] = eig(A-B*K)% T= eig_vector e=eig_value

[eigvector,eigvalue] = eig(A-B*K)% T= eig_vector e=eig_value
diageigval = diag(real(eigvalue)) % eig_value

first_eigvector = eigvector(:,1)

%subplot(2,2,1), plot(fig1) %在2x2之四個圖中，繪第1個小圖
%subplot(2,2,2), plot(fig2) %在2x2之四個圖中，繪第2個小圖
%subplot(2,2,3), plot(fig3) %在2x2之四個圖中，繪第3個小圖
%subplot(2,2,4), plot(fig4) %在2x2之四個圖中，繪第4個小圖

% function dy = pendcart(y,m,M,L,g,d,u)

#作業二