-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPC2Plot.m
156 lines (121 loc) · 4.26 KB
/
PC2Plot.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
clear
clc
close all
% Solve DE numerically using ode45
m_num = -3;
b_num = 5;
l_num = 3;
h_num = 10;
k_num = 1;
m_b_num = 0.5;
m_c_num = 0.5;
S_num = 10;
theta_num = atan(-m_num);
z_0_num = 2;
phi_0_num = pi/4;
g_num = 9.81;
tspan = [0 7];
Y0 = [z_0_num 0 phi_0_num 0];
options = odeset('RelTol',1e-6);
% Use created .m file to solve DE
[t, Y] = ode45(@PC2_sys,tspan,Y0,options,m_num,b_num,l_num,h_num,k_num,m_b_num,m_c_num,g_num,S_num);
% Position vectors
r_c = @(z)[-z*cos(theta_num)-(b_num/m_num);z*sin(theta_num)];
r_b = @(z, phi) r_c(z) + [h_num*sin(theta_num) + l_num*sin(phi-theta_num); h_num*cos(theta_num)-l_num*cos(phi-theta_num)];
% r_k = @(rz) [rz*cos(theta_num)+b_num/m_num;b_num-rz*sin(theta_num)];
r_z = @(z) [-z*cos(theta_num);z*sin(theta_num)];
lineFun = @(xx) (m_num * xx + b_num);
% Draw the figure
figure;
% Start recording
myVideo = VideoWriter('system_3', 'MPEG-4'); %open video file
myVideo.FrameRate = 10; %can adjust this, 5 - 10 works well for me
open(myVideo)
% Plot options (change if necessary)
DEBUG = false;
PLOT_POS_VECTORS = false;
axis('equal');
% Wipe the slate clean
n_points = length(Y(:,1))
for k=1:n_points
% Wipe the slate clean
clf
% Plot inclined plane
LINE_SPACE_SIZE = 10;
line_x = linspace(-30, 30, LINE_SPACE_SIZE);
line_y = lineFun(line_x);
plot(line_x, line_y, 'k', 'LineWidth', 3)
% Plot 'wall'
hold on
plot(line_x, -line_y, 'k', 'LineWidth', 3)
% Plot 'floor'
hold on
plot(line_x, zeros(LINE_SPACE_SIZE), 'k', 'LineWidth', 3)
% Calculate position vectors
z = Y(k,1);
phi = Y(k,3);
posC = r_c(z);
posB = r_b(z, phi);
% Body C
% plot(posC(1), posC(2), 's', 'MarkerSize', 30, 'MarkerEdgeColor', 'black', 'MarkerFaceColor', '#BBBBBB')
% Draw stick of height h_num and width w_stick
w_stick = 0.25;
A_1 = [posC(1) - w_stick; lineFun(posC(1) - w_stick)];
A_2 = [posC(1) + w_stick; lineFun(posC(1) + w_stick)];
B_1 = A_1 + h_num * [sin(theta_num); cos(theta_num)];
B_2 = A_2 + h_num * [sin(theta_num); cos(theta_num)];
hold on
fill([A_1(1) B_1(1) B_2(1) A_2(1)], [A_1(2) B_1(2) B_2(2) A_2(2)], 'black', 'FaceAlpha', 0.3);
% Draw string of length l between the middle of the stick and rB
plot([(B_1(1) + B_2(1))/2, posB(1)], [(B_1(2) + B_2(2))/2, posB(2)], 'black', 'LineWidth', 2)
% Draw rectangle
w_rect = 2;
h_rect = 3;
A_1 = [posC(1) - w_rect; lineFun(posC(1) - w_rect)];
A_2 = [posC(1) + w_rect; lineFun(posC(1) + w_rect)];
B_1 = A_1 + h_rect * [sin(theta_num); cos(theta_num)];
B_2 = A_2 + h_rect * [sin(theta_num); cos(theta_num)];
hold on
fill([A_1(1) B_1(1) B_2(1) A_2(1)], [A_1(2) B_1(2) B_2(2) A_2(2)], [0.6 0.6 0.6]);
% Draw spring
NE = 10;
NR = k_num;
[x_spring, y_spring] = spring((A_2(1) + B_2(1)) / 2, (A_2(2) + B_2(2)) / 2, -b_num/m_num, 0, NE, S_num, NR);
plot(x_spring,y_spring,'black','LineWidth',2);
% Body B
hold on
circles(posB(1), posB(2), 0.5, 'color', 'black', 'FaceAlpha', 0.4);
if (PLOT_POS_VECTORS)
quiver(0, 0, posC(1), posC(2), 'Color', '#0072BD', 'LineWidth', 2);
quiver(0, 0, posB(1), posB(2), 'Color', '#7E2F8E', 'LineWidth', 2);
end
if (DEBUG)
% r0 vector
hold on
quiver(0, 0, -b_num/m_num, 0, 'Color', '#A2142F', 'LineWidth', 2);
% rk vector
% posK = r_k(Y(k,1));
% hold on
% quiver(-b_num/m_num, 0, posK(1), posK(2), 'Color', '#EDB120', 'LineWidth', 2);
% ry vector
hold on
posY = [0;b_num];
quiver(0, 0, posY(1), posY(2), 'Color', '#77AC30', 'LineWidth', 2);
% rz vector
posZ = r_z(Y(k,1));
hold on
quiver(posY(1), posY(2), posZ(1), posZ(2), 'Color', '#00FFFF', 'LineWidth', 2);
end
% Decorate the plot
grid on
xlabel('x')
ylabel('y')
title([sprintf("$m = %0.5f$, $z_0 = %0.5f$, $\\varphi_0 = %0.5f$", m_num, z_0_num, phi_0_num)], 'Interpreter','latex')
text(17, 26, sprintf('$z = %0.5f$\n$\\varphi = %0.5f$', z, phi), 'Interpreter','latex')
% Force matlab to draw the image
drawnow
% pause(0.001) %Pause and grab frame
frame = getframe(gcf); % get frame
writeVideo(myVideo, frame);
end
close(myVideo)