matlab 作業 工科系111級 林洵 E94076063

# **作業1**

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 30 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+.45; 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
code部分

title('step response with digital lqr control')
x=y(:,1);
x_dot=y(:,2);
theta=y(:,3);
theta_dot=y(:,4);
subplot(4, 1, 1); plot(t, x);
xlabel('time')
ylabel('x')
grid on
hold on
subplot(4, 1, 2); plot(t, x_dot);
xlabel('time')
ylabel('x dot')
grid on
hold on
subplot(4, 1, 3); plot(t, theta);
xlabel('time')
ylabel('theta')
grid on
hold on
subplot(4, 1, 4); plot(t, theta_dot);
xlabel('time')
ylabel('theta dot')
grid on
hold on

圖表呈現


# 作業3
#
code部分

y0 = [-3; 0; pi+.5; 0];
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_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
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_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
x_dis_1
dx_dis_1
balance_time 
angle_2

結果

X的距離:-4.8301
速度:-5.6199
平衡時間:2.0831
角度:0.0902