sravan/Control-Project
1
0
Fork 0
mirror of https://github.com/ROB-535-F21-Team-3/Control-Project.git synced 2025-08-23 10:52:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

Re-Organize Files Again

Browse Source 
- Deliverables folder holds final files for submission
- Experimentation holds scripts and things related to our development of
  a final solution
- Simulation holds the provided files
- Added template for part 2 submission with comments
This commit is contained in:
Sravan Balaji
2021-11-30 11:17:24 -05:00
parent b0ad7defd3
commit 33945f6414
15 changed files with 40 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

206
Experimentation/xenia_final_project.m Normal file
View File

@@ -0,0 +1,206 @@
%Vehicle Parameterrs
Nw=2;
f=0.01;
Iz=2667;
a=1.35;
b=1.45;
By=0.27;
Cy=1.2;
Dy=0.7;
Ey=-1.6;
Shy=0;
Svy=0;
m=1400;
g=9.806;
%note constraints on input
%delta [-0.5, 0.5]
%Fx [-5000, 5000]
init = [287, 5, -176, 0, 2, 0];
curr_xy = [287, -176];
U_try = [0.4, 5000];
for i = 1:12
U_try = [U_try; U_try];
end
U_final = [];
U_try = [0.4, 5000; 0.4, 5000];
[Y, T] = forwardIntegrateControlInput(U_try);
traj_inf = getTrajectoryInfo(Y, U_try);
%traj_inf.t_finished
init = [287, 5, -176, 0, 2, 0];
U_try = [0.25, 2500; 0.25, 2500; 0.25, 2500];
noise = [randn*0.01, randn*10];
U_try = U_try + noise;
[Y, T] = forwardIntegrateControlInput(U_try);
traj_inf = getTrajectoryInfo(Y, U_try);
U_final = U_try;
max_percent = 0;
num_iter = 0;
subt = 0;
randomize = 0;
percent_comp = 0;
avgcount = [];
while (size(traj_inf.t_finished) == 0)
%for i = 1:100
noise = [randn*0.01, randn*10];
U_try = U_try + noise;
[Y_temp, T] = forwardIntegrateControlInput(U_try, Y(end,:));
traj_inf = getTrajectoryInfo([Y;Y_temp], [U_final; U_try]);
if mod(num_iter+1, 11) == 0 | traj_inf.percent_of_track_completed < (percent_comp - 0.0001)
randomize = 1;
end
%if crashes, redo, if not add on and keep going
%also will allow it to change inputs once in a while for randomization
%next thing to onsider trying is the intersectLineSegment thing and
%following centerline
count = 1;
factor = 3;
while size(traj_inf.left_track_position) ~= 0 | randomize == 1
if subt == 0
U_og = U_try;
U_try = U_try + [0.011, 250];
if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
subt = 1;
end
if U_try(1,1) > 0.45
U_try = U_try - [0.05, 0];
end
if U_try(1,2) > 4500
U_try = U_try - [0, 660];
end
noise = [randn*0.01, randn*10];
U_try = U_try + noise;
end
if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
subt = 1;
[Y_comp1, T] = forwardIntegrateControlInput(U_try, Y(end,:));
U_try = U_og;
end
if subt == 1
if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
subt = 0;
end
U_try = U_try - [0.011, 250];
if U_try(1,1) < -0.45
U_try = U_try + [0.05, 0];
end
if U_try(1,2) < -4500
U_try = U_try + [0, 660];
end
noise = [randn*0.01, randn*10];
U_try = U_try + noise;
end
if factor > (size(Y,1)/3) || (abs(U_try(1,1)) > 0.45 && abs(U_try(1,2)) > 4700)
[Y_comp2, T] = forwardIntegrateControlInput(U_try, Y(end,:));
for i = 1:(246/3)
dist1 = norm([Y_comp1(:,1)'; Y_comp1(:,3)'] - TestTrack.cline(:,i:i+2));
dist2 = norm([Y_comp2(:,1)'; Y_comp2(:,3)'] - TestTrack.cline(:,i:i+2));
diff1 = min(dist1);
diff2 = min(dist2);
if diff1 < 20 | diff2 < 20
break
end
end
if diff1 > diff2
subt = 0;
end
if diff2 > diff1
subt = 1;
end
U_try = U_og;
if subt == 0
U_try = U_try + [0.011, 250];
% if (U_try(1,1) > 0.49 && U_try(1,2) > 4900)
% subt = 1;
% end
if U_try(1,1) > 0.5
U_try = U_try - [0.05, 0];
end
if U_try(1,2) > 4000
U_try = U_try - [0, 760];
end
end
if subt == 1
% if (U_try(1,1) < -0.49 && U_try(1,2) < -4900)
% subt = 0;
% end
U_try = U_try - [0.011, 250];
if U_try(1,1) < -0.5
U_try = U_try + [0.05, 0];
end
if U_try(1,2) < -4000
U_try = U_try + [0, 760];
end
end
end
%instead we'll go back one step and so we'll send 6 commands and
%check
if mod(count,14) == 0
factor = factor + 3;
end
if factor > 5
enteredifstatementon166 = 1
break
break
end
U_try(:,1) = min(max(U_try(:,1), -0.5), 0.5);
U_try(:,2) = min(max(U_try(:,2), -5000), 5000);
U_try2 = U_try;
for j = 1:(factor/3)
U_try2 = [U_try2; U_try];
end
[Y_temp, T] = forwardIntegrateControlInput(U_try2, Y(end-factor,:));
traj_inf = getTrajectoryInfo([Y(1:end-factor,:);Y_temp], [U_final(1:end-factor,:); U_try2]);
if size(traj_inf.left_track_position) == 0
U_final = U_final(1:end-factor,:);
Y = Y(1:end-factor,:);
end
count = count + 1;
randomize = 0;
end
if count ~= 1
avgcount = [avgcount ; count];
end
if (traj_inf.percent_of_track_completed > percent_comp)
percent_comp = traj_inf.percent_of_track_completed;
end
U_final = [U_final; U_try];
Y = [Y; Y_temp(1:3,:)];
num_iter = num_iter + count
end