**matlab homework 汪柏安**
---------------
# HW1

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 = [30 0 0 0;
    0 500000 0 0;
    0 0 50 0;
    0 0 0 50000];
R = .0002;

%%
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

# HW2
# HW3

x_dis_1=-1;
a=1;
for a=1:1:length(t)
    x_dis=y(a,1)
   if x_dis_1>x_dis
    x_dis_1=x_dis;
   else
    x_dis=x_dis;
   end
end

x_dis_1=-4.7728

dx_dis_1=0;
a=1;
for a=1:1:length(t)
    dx_dis=y(a,2)
   if dx_dis_1>dx_dis
    dx_dis_1=dx_dis;
   else
    dx_dis=dx_dis;
   end
end   

dx_dis_1=-4.2479

theda_dis_1=pi+0.4;
a=1;
for a=1:1:length(t)
    theda_dis=y(a,3)
   if theda_dis_1<theda_dis
    theda_dis_1=theda_dis;
   else
    theda_dis=theda_dis;
   end
end

theda_dis_1=3.5416

a=1;
dtheda_dis_1=0;
for a=1:1:length(t)
    dtheda_dis = y(a,4);
        dtheda_dis_1=dtheda_dis;
    end
end

dtheda_dis_1=0.0606

balance=0;
for a=1:1:length(t)
     if (y(a,3)-pi)>pi*0.001
        balance=balance+1;
     end
end
balance_time=balance/1000;
angle=(theda_dis_1-pi)*180/pi;

angle=22.9183
balance_time=0.377