# Matlab作業

# 作業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)];
eig(A)

C = [1 0 0 0;
     0 1 0 0;
     0 0 1 0;
     0 0 0 1];
 
D = [0;
     0;
     0;
     0];
Q = [8 0 0 0;
    0 10 0 0;
    0 0 5 0;
    0 0 0 9];   
R = .0000001;

%%
det(ctrb(A,B))

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

[eig_vector,eig_value]=eig(A-B*K) % T=vector e=value
diag_eigval = diag(real(eig_value)) %value
%最穩定value之vector
first_eigvector = eig_vector(:,1) %most stable

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+.41; 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-[0; 0; pi; 0])),tspan,y0);
else
    
end

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

# 作業2


# 作業3

**初始角度**

 y0 = [-3; 0; pi+.41; 0];

**x距離的最大值(程式碼與最大值)**

...
x_dis_1=-1;
for a=1:1:length(t)
    x_dis=y(a,1);
    if x_dis_1 >x_dis
        x_dis_1 =x_dis;
        a_time = a/1000;
    end
end
...


**dx速度的最大值(程式碼與最大值)**

...
dx_dis_1=1;
for a=1:1:length(t)
    dx_dis=y(a,2);
    if dx_dis_1>dx_dis
        dx_dis_1=dx_dis;
        a_time = a/1000;
    end
end
...


**theta角度總偏離角度與平衡時間(程式碼與偏離角度還有時間)**

...
angle=0;
for a=1:1:length(t)
    t_dis=y(a,3);
    if angle<t_dis-pi
        angle=t_dis-pi;
        a_time = a/1000;
    end
end
angle=angle*(180/pi);

balancetime=5;
for a=1:1:(length(t)-10)
    if abs(y(a+10,3)-y(a,3))<0.0001 && y(a,3)>3.11
        balancetime=t(a);
        break;
    end
end 
...


**dtheta角速度的最大值(程式碼與最大值)**

dt_dis_1=1;
for a=1:1:length(t)
    dt_dis=y(a,4);
    if dt_dis_1>dt_dis
        dt_dis_1=dt_dis;
        a_time = a/1000;
    end
end


**總結果**