# 線代作業

## 作業一

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 1 0 0;
    0 0 10 0;
    0 0 0 100];
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+.47; 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 

## 作業二

## 作業三
### 初始角度

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

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

 a = 1 ;
x_dis_1=-1;
for a=1:1:length(t)
    x = y(a,1);
    if x < x_dis_1
        x_dis_1 = x;
    end
end
fprintf('x_dis_1 = %d\n',x_dis_1)


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

b = 1 ;
xdot_dis_1=-1;
for b=1:1:length(t)
    xdot = y(b,2);
    if xdot < xdot_dis_1
        xdot_dis_1 = xdot;
    end
end
fprintf('xdot_dis_1 = %d\n',xdot_dis_1)


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

%平衡時間
for e=1:1:length(t)
    a1 = y(e,3);
    t3 = 0;
    if a1-pi < 0.001 
       t3 = t(e,1);
       break;
    end
end
fprintf('balance time = %d\n',t3)

%偏離角度
theta_dis_1 = pi+.47;
for c=1:1:length(t)
    theta = y(c,3);
    if theta_dis_1 < theta
        theta_dis_1 = theta;
    end
end
theta_dis_2=(theta_dis_1-pi)*180/pi;
fprintf('angle = %d\n',theta_dis_2)


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

thetadot_dis_1=0;
for d=1:1:length(t)
    thetadot = y(d,4);
    if thetadot^2 > thetadot_dis_1^2
        thetadot_dis_1 = thetadot;
    end
end
fprintf('thetadot_dis_1 = %d\n',thetadot_dis_1)

### 總結果