forked from kshitijgoel007/AUVForwardDynamics
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathrunSimInLoop.m
158 lines (141 loc) · 4.1 KB
/
runSimInLoop.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
function runSimInLoop()
% Input function for specifying which maneuver to perform
% u =1,1.5,2m/s
% CASE1: TURNING DEL_R_ORDERED=10DEGREE
% CASE2: TURNING DEL_R_ORDERED=15DEGREE
% CASE3: HORIZONTAL ZIG-ZAG MOTION: DEL_R_ORDERED=10DEGREE INITIALLY
% CASE4: HORIZONTAL ZIG-ZAG MOTION: DEL_R_ORDERED=-10DEGREE INTIALLY
% CASE5: HORIZONTAL ZIG-ZAG MOTION: DEL_R_ORDERED=20DEGREE INITIALLY
% CASE6: HORIZONTAL ZIG-ZAG MOTION: DEL_R_ORDERED=-20DEGREE INITIALLY
% CASE7: VERTICAL ZIG-ZAG MOTION: DEL_ST_ORDERED= 10DEGREE INITIALLY
% CASE8: VERTICAL ZIG-ZAG MOTION: DEL_ST_ORDERED=-10DEGREE INITIALLY
% CASE9: STEERING PD CONTROLLER
% CASE10: DIVING PD CONTROLLER
% CASE 11: TURNING DEL_R_ORDERED = -10 DEGREE,
% CASE 12: TURNING DEL_R_ORDERED = -15 DEGREE,
% CASE 13: VERTICAL ZIG-ZAG MOTION: DEL_ST_ORDERED = 20DEGREE INITIALLY,
% CASE 14: VERTICAL ZIG-ZAG MOTION: DEL_ST_ORDERED = -20 DEGREE INITIALLY
clc;
close all;
addpath('actuator dynamics');
addpath('utils');
addpath('PDcontrol');
addpath('Solvers');
addpath('excel data');
addpath('excel data/u_1m_s');
addpath('excel data/u_1dot5m_s');
addpath('excel data/u_2m_s');
r2d =180/pi;
lbl = {'t(s)', 'u(m/s)','v(m/s)','w(m/s)',...
'p(deg/s)','q(deg/s)','r(deg/s)'...
'X(m)','Y(m)','Z(m)','phi(deg)','theta(deg)','psi(deg)'...
'Del_s(deg)','Del_bp(deg)','Del_bs(deg)','Del_r(deg)',...
'Del_delb',...
'n(RPS)','ax(m/s2)','ay(m/s2)','az(m/s2)',...
'alphax(deg/s2)','alphay(deg/s2)','alphaz(deg/s2)'};
%setting solver to rk4t
sCheck =1;
% setting time span
tspan = 0:.1:100;
% code for running additional cases
% ...................%u = 1m/s.............................................
X = zeros(18,1);
X(18) = 110.969;
X(1) = 1;
%running simulation
parfor i = 1:2
caseNo = num2str(i);
Y=callSolver(caseNo,tspan,X,sCheck);
%SAVING Y VECTOR
%angle conversion to degrees
temp = Y;
temp(:,5:7) = temp(:,5:7)*r2d;
temp(:,11:17)= temp(:,11:17)*r2d;
temp(:,23:25) = temp(:,23:25)*r2d;
%writing labels
fname1=['excel data/u_1m_s/case_',caseNo,'.csv'] ;
fid = fopen(fname1,'w');
[rows,cols]=size(lbl);
for k=1:rows
fprintf(fid,'%s,',lbl{k,1:end-1});
fprintf(fid,'%s\n',lbl{k,end});
end
fclose(fid);
fid=fopen(fname1,'a') ;
dlmwrite(fname1,temp,'-append','precision',4);
fclose(fid);
end
end
% %............................u = 1.5m/s................................
% X = zeros(18,1);
% X(18) = 163;
% X(1) = 1.5;
%
%
%
% %running simulation
% for i = 1:14
% caseNo =num2str(i);
% Y=callSolver(caseNo,tspan,X,sCheck);
%
% %SAVING Y VECTOR
% %angle conversion to degrees
% temp = Y;
% temp(:,5:7) = temp(:,5:7)*r2d;
% temp(:,11:17)= temp(:,11:17)*r2d;
% temp(:,23:25) = temp(:,23:25)*r2d;
%
%
% %writing labels
% fname1=['excel data/u_1dot5m_s/case_',caseNo,'.csv'] ;
% fid = fopen(fname1,'w');
% [rows,cols]=size(lbl);
% for k=1:rows
% fprintf(fid,'%s,',lbl{k,1:end-1});
% fprintf(fid,'%s\n',lbl{k,end});
% end
% fclose(fid);
%
% fid=fopen(fname1,'a') ;
% dlmwrite(fname1,temp,'-append','precision',4);
% fclose(fid);
%
% end
%
% ........................u = 2m/s..............................
% X = zeros(18,1);
% X(18) = 215.3;
% X(1) = 2;
%
%
%
%
% %running simulation
% for i = 1:14
% caseNo =num2str(i);
% Y=callSolver(caseNo,tspan,X,sCheck);
%
% %SAVING Y VECTOR
% %angle conversion to degrees
% temp = Y;
% temp(:,5:7) = temp(:,5:7)*r2d;
% temp(:,11:17)= temp(:,11:17)*r2d;
% temp(:,23:25) = temp(:,23:25)*r2d;
%
%
% %writing labels
% fname1=['excel data/u_2m_s/case_',caseNo,'.csv'] ;
% fid = fopen(fname1,'w');
% [rows,cols]=size(lbl);
% for k=1:rows
% fprintf(fid,'%s,',lbl{k,1:end-1});
% fprintf(fid,'%s\n',lbl{k,end});
% end
% fclose(fid);
%
% fid=fopen(fname1,'a') ;
% dlmwrite(fname1,temp,'-append','precision',4);
% fclose(fid);
%
% end
%