forked from burakbayramli/books
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathElasticTRBDF.m
173 lines (134 loc) · 6.76 KB
/
ElasticTRBDF.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
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
function ElasticTRBDF(T,dt,theta_init,thetadot_init,r_init,rdot_init,label)
%
%Elastic(T,dt,powers)
%-----------------------------------------------------------------------------------
%Plots how the angles theta1 theta2 and their derivatives vary in time for
%the ELASTIC pendulum using the Trapezoidal scheme - assuming L=1, k=10,
m=1
%Animated the elastic pendulum system with desired parameters using the TR-BDF2 Rule
% @ Sohan Dharmaraja MIT JUN2007
dt = dt/2;
powers = 1;
k=10;
m=1;
L=1;
timevec(1)=0;
thetavec(1)=theta_init;
thetadotvec(1)=thetadot_init;
rvec(1)=r_init;
rdotvec(1)=rdot_init;
xold=[theta_init; thetadot_init; r_init; rdot_init];
xnew=xold;
N=T/dt;
iter=1;
timevec(1)=0;
thetavec(1)=theta_init;
thetadotvec(1)=thetadot_init;
rvec(1)=r_init;
rdotvec(1)=rdot_init;
xold=[theta_init; thetadot_init; r_init; rdot_init];
xnew=xold;
f1=inline('iternew1 - (dt/2)*iternew2 - orig1 - (dt/2)*orig2','dt','orig1','orig2','iternew1','iternew2');
f2=inline('iternew2 - (dt/2)*((2*iternew4*iternew2)/((-1)*iternew3) + ((9.81/((-1)*iternew3))*sin(iternew1))) - orig2 - (dt/2)*((2*orig4*orig2)/((-1)*orig3) + (((9.81/(-1*orig3)))*sin(orig1)))','dt','orig1','orig2','orig3','orig4','iternew1','iternew2','iternew3','iternew4');
f3=inline('iternew3 - (dt/2)*iternew4 - orig3 - (dt/2)*orig4','dt','orig3','orig4','iternew3','iternew4');
f4=inline('iternew4 - (dt/2)*((9.81*cos(iternew1) - (k/(m))*(iternew3-L) + iternew3*((iternew2)^2))) - orig4 - (dt/2)*((9.81*cos(orig1) - (k/(m))*(orig3-L) + orig3*((orig2)^2)))','dt','orig1','orig2','orig3','orig4','iternew1','iternew2','iternew3','iternew4','k','L','m');
%define the 3pt BE scheme
f1BE=inline('iternew1 - (4/3)*orig1older +(1/3)*orig1oldest - (2*dt/3)*iternew2','dt','orig1older','orig1oldest','iternew1','iternew2');
f2BE=inline('iternew2 - (4/3)*orig2older +(1/3)*orig2oldest - (2*dt/3)*((2*iternew4*iternew2)/((-1)*iternew3) + ((9.81/((-1)*iternew3))*sin(iternew1)))','dt','orig2older','orig2oldest','iternew1','iternew2','iternew3','iternew4');
f3BE=inline('iternew3 - (4/3)*orig3older +(1/3)*orig3oldest - (2*dt/3)*iternew4','dt','orig3older','orig3oldest','iternew3','iternew4');
f4BE=inline('iternew4 - (4/3)*orig4older +(1/3)*orig4oldest - (2*dt/3)*((9.81*cos(iternew1) - (k/(m))*(iternew3-L) + iternew3*((iternew2)^2)))','dt','orig4older','orig4oldest','iternew1','iternew2','iternew3','iternew4','k','m','L');
posvec = 1;
for iter=1:2:N-1
posvec = posvec + 1;
xoldest = xnew;
xinit=xnew;
for j=1:5
theta=xnew(1,1);
V=xnew(2,1);
r=xnew(3,1);
W=xnew(4,1);
J = [1, -1*(dt/2), 0, 0; (9.81/r)*cos(theta)*(dt/2), 1+((W/r)*dt), -1*(dt/2)*((9.81/(r^2))*sin(theta)+ (2*W*V)/(r^2)), V*(dt/r); 0, 0, 1, (-1)*(dt/2); (9.81*sin(theta))*(dt/2), (-1)*r*V*dt, (-1)*(V^2)*(dt/2)+(k/(2*m))*dt, 1];
RHSvec(1,1)=f1(dt,xinit(1,1),xinit(2,1),xold(1,1),xold(2,1));
RHSvec(2,1)=f2(dt,xinit(1,1),xinit(2,1),xinit(3,1),xinit(4,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1));
RHSvec(3,1)=f3(dt,xinit(3,1),xinit(4,1),xold(3,1),xold(4,1));
RHSvec(4,1)=f4(dt,xinit(1,1),xinit(2,1),xinit(3,1),xinit(4,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1),k,L,m);
deltax=(J)\((-1)*RHSvec);
xnew=xold+deltax;
xold=xnew;
% =====================================================================
% =====================================================================
xolder = xnew;
for dotimes=1:5
a=xnew(1,1);
b=xnew(2,1);
c=xnew(3,1);
d=xnew(4,1);
BETempmat=zeros(4,4);
%Fill in the Jacobian element wise - easier debugging!
BETempmat(1,1)=1;
BETempmat(1,2)=-(2*dt/3);
BETempmat(2,1)=-(2*dt/3)*(((9.81/((-1)*c))*cos(a)));
BETempmat(2,2)=1-(2*dt/3)*((2*d*b)/((-1)*c));
BETempmat(2,3)=(2*dt/3)*((2*d*b)/(c^2) + ((9.81/(c^2))*sin(a)));
BETempmat(2,4)=(2*dt/3)*((2*b)/((-1)*c));
BETempmat(3,3)=1;
BETempmat(3,4)=-(2*dt/3);
BETempmat(4,1)=(2*dt/3)*((9.81*sin(a)));
BETempmat(4,2)=-(2*dt/3)*((2*c*((b))));
BETempmat(4,3)=-(2*dt/3)*((- (k/m)*(c) + ((b)^2)));
BETempmat(4,4)=1;
RHSvec(1,1)=f1BE(dt,xolder(1,1),xoldest(1,1),xold(1,1),xold(2,1));
RHSvec(2,1)=f2BE(dt,xolder(2,1),xoldest(2,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1));
RHSvec(3,1)=f3BE(dt,xolder(3,1),xoldest(3,1),xold(3,1),xold(4,1));
RHSvec(4,1)=f4BE(dt,xolder(4,1),xoldest(4,1),xold(1,1),xold(2,1),xold(3,1),xold(4,1),k,m,L);
deltax=(BETempmat)\((-1)*RHSvec);
xnew=xold+deltax;
xold=xnew;
end
% =====================================================================
% =====================================================================
end
thetavec(posvec)=xnew(1,1);
thetadotvec(posvec)=xnew(2,1);
rvec(posvec)=xnew(3,1);
rdotvec(posvec)=xnew(4,1);
timevec(posvec)=iter*dt;
subplot(2,2,1)
plot(timevec,thetavec,'LineWidth',2)
title('theta vs time','FontWeight','bold')
hold on
subplot(2,2,2)
plot(timevec,thetadotvec,'LineWidth',2)
title('theta dot vs time','FontWeight','bold')
hold on
subplot(2,2,3)
plot(timevec,rvec,'LineWidth',2)
title('r vs time','FontWeight','bold')
hold on
subplot(2,2,4)
plot(timevec,rdotvec,'LineWidth',2)
title('r dot vs time','FontWeight','bold')
hold on
store=[thetavec', thetadotvec'];
end
% now plot the exact solution using Matlab
% ------------------------------------------------------------------------------------------------- change this value below for spring constant
%g = inline('[ y(2); (2*y(2)*y(4))/((-1*y(3))) + (9.81 *sin(y(1)))/(-1*y(3));y(4); 9.81*cos(y(1)) - ((k/m)*(y(3)-1)) + y(3)*((y(2))^2)]', 't','y','k','m');
%------------------------------------------------------------------------------------------------- ((k/m))*(R - Lnatural) change this value below for spring constant
g = inline('[ y(2); (2*y(2)*y(4))/((-1*y(3))) + (9.81 *sin(y(1)))/(-1*y(3));y(4); 9.81*cos(y(1)) - ((10)*(y(3)-1)) + y(3)*((y(2))^2)]', 't', 'y');
[t,y]=ode45(g,[0,T],[theta_init; thetadot_init; r_init; rdot_init]);
% Can do - [t,y]=ode45(g,[0:0.1:2*pi],[theta_init;0]); if diff time steps required
subplot(2,2,1)
plot(t,y(:,1),'Color','red', 'LineWidth',2)
hold on
subplot(2,2,2)
plot(t,y(:,2),'Color','red', 'LineWidth',2)
hold on
subplot(2,2,3)
plot(t,y(:,3),'Color','red', 'LineWidth',2)
hold on
subplot(2,2,4)
plot(t,y(:,4),'Color','red', 'LineWidth',2)
hold off
initinfo = [num2str(T), ' ', num2str(2*dt), ' ', num2str(theta_init),' ',num2str(thetadot_init), ' ', num2str(r_init),' ', num2str(rdot_init)];
saveas(gcf,[num2str(label) ' TRBDF - ' initinfo '.png']);close;