# Matlab 作業:
### B34061306 黃顗中

## 作業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;
    0 33 0 0;
    0 0 4 0;
    0 0 0 1];
R = .0001;

%%
det(ctrb(A,B))

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

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

first_eigvector = eig_vector(:,1)

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

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

## 作業2

## 作業3:
### 初始角度

  y0 = [-3; 0; pi+.5; 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;
    end
end


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

angle =0;
balance_time =0;
for a = 1:1:length(t)
   angle_1 = abs(y(a,3)-(pi));
   angle_2 = angle_1*(180/pi);
   if angle_2 > 1
       balance_time =t(a);
   end
   if angle < angle_1
       angle = angle_1;
   end
end


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

dtheta_dis_2 = -1;
for a = 1:1:length(t)
    dtheta_dis = y(a,4);
    if dtheta_dis < dtheta_dis_2
        dtheta_dis_2 = dtheta_dis;
    end
end


### 總結果