mirror of
https://github.com/ROB-535-F21-Team-3/Control-Project.git
synced 2025-08-23 19:02:44 +00:00
just committing bc git confuses me
This commit is contained in:
xdemenchuk
@@ -184,6 +184,27 @@ info = getTrajectoryInfo(Y,U)
|
|||||||
[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
|
[Y_submission, T_submission] = forwardIntegrateControlInput(ROB535_ControlProject_part1_input, state_0);
|
||||||
info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)
|
info = getTrajectoryInfo(Y_submission,ROB535_ControlProject_part1_input)
|
||||||
|
|
||||||
|
index_cl_cp = 1;
|
||||||
|
index_cl_nextcp = 2;
|
||||||
|
%xenia test
|
||||||
|
for i = 1:size(Y,1)
|
||||||
|
|
||||||
|
curr_xy = [Y(i,1); Y(i,3)];
|
||||||
|
dist2cp1 = norm(curr_xy - TestTrack.cline(:, index_cl_cp));
|
||||||
|
dist2cp2 = norm(curr_xy - TestTrack.cline(:, index_cl_nextcp));
|
||||||
|
|
||||||
|
if (dist2cp2 < dist2cp1)
|
||||||
|
i
|
||||||
|
dist2cp1
|
||||||
|
dist2cp2
|
||||||
|
index_cl_cp = index_cl_cp + 1;
|
||||||
|
index_cl_nextcp = index_cl_nextcp + 1;
|
||||||
|
if (index_cl_nextcp > size(TestTrack.cline, 2))
|
||||||
|
break
|
||||||
|
end
|
||||||
|
end
|
||||||
|
end
|
||||||
|
|
||||||
%% Figures
|
%% Figures
|
||||||
% Plot segmented trajectory for debugging purposes
|
% Plot segmented trajectory for debugging purposes
|
||||||
figure(1)
|
figure(1)
|
||||||
|
|||||||
@@ -1,281 +0,0 @@
|
|||||||
%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]
|
|
||||||
|
|
||||||
load("TestTrack.mat");
|
|
||||||
|
|
||||||
init = [287, 5, -176, 0, 2, 0];
|
|
||||||
curr_pos = [init(1);init(3)];
|
|
||||||
|
|
||||||
%generate bounds, will need to index these appropriately for obstacle
|
|
||||||
%avoidance
|
|
||||||
[LB, UB] = bounds(10);
|
|
||||||
|
|
||||||
Nobs = 25;
|
|
||||||
Xobs = generateRandomObstacles(Nobs);
|
|
||||||
|
|
||||||
%state_size = tbd;
|
|
||||||
z = [init, init, -0.004, 3900, -0.004, 3900]; %for testing purposes
|
|
||||||
|
|
||||||
[g,h,dg,dh]=nonlcon(z, Xobs);
|
|
||||||
|
|
||||||
function [lb, ub] = bounds(StepsPerPoint)
|
|
||||||
load('TestTrack.mat');
|
|
||||||
|
|
||||||
[m,nPts] = size(TestTrack.cline);
|
|
||||||
% numState = 6;
|
|
||||||
% numInput = 2;
|
|
||||||
nsteps = StepsPerPoint * nPts;
|
|
||||||
|
|
||||||
lb_u = [-0.5;-5000];
|
|
||||||
ub_u = [0.5;5000];
|
|
||||||
|
|
||||||
bound_X = [TestTrack.bl(1,1), TestTrack.bl(1,2), ...
|
|
||||||
TestTrack.br(1,1), TestTrack.br(1,2)];
|
|
||||||
bound_Y = [TestTrack.bl(2,1), TestTrack.bl(2,2), ...
|
|
||||||
TestTrack.br(2,1), TestTrack.br(2,2)];
|
|
||||||
phi_init = TestTrack.theta(1);
|
|
||||||
|
|
||||||
%phi restricted to just [-pi/2 pi/2]
|
|
||||||
lb = [min(bound_X); -Inf; min(bound_Y); -Inf; -(pi/2); -Inf];
|
|
||||||
ub = [max(bound_X); Inf; max(bound_Y); Inf; +(pi/2); Inf];
|
|
||||||
|
|
||||||
|
|
||||||
for i=1:nPts
|
|
||||||
prev_idx = max(i-1, 1);
|
|
||||||
next_idx = min(i+1, 246);
|
|
||||||
|
|
||||||
bound_X = [TestTrack.bl(1,prev_idx), TestTrack.bl(1,next_idx), ...
|
|
||||||
TestTrack.br(1,prev_idx), TestTrack.br(1,next_idx)];
|
|
||||||
bound_Y = [TestTrack.bl(2,prev_idx), TestTrack.bl(2,next_idx), ...
|
|
||||||
TestTrack.br(2,prev_idx), TestTrack.br(2,next_idx)];
|
|
||||||
phi_init = TestTrack.theta(i);
|
|
||||||
|
|
||||||
lb_x = [min(bound_X); -Inf; min(bound_Y); -Inf; -(pi/2); -Inf];
|
|
||||||
ub_x = [max(bound_X); Inf; max(bound_Y); Inf; +(pi/2); Inf];
|
|
||||||
|
|
||||||
for num = 1:StepsPerPoint
|
|
||||||
lb=[lb;lb_x];
|
|
||||||
ub=[ub;ub_x];
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
for i=1:nsteps
|
|
||||||
ub=[ub;ub_u];
|
|
||||||
lb=[lb;lb_u];
|
|
||||||
end
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
function [g, h, dg, dh] = nonlcon(z, Xobs)
|
|
||||||
|
|
||||||
nsteps = (size(z,2)/8);
|
|
||||||
curr_pos = [z(1); z(3)];
|
|
||||||
|
|
||||||
Xobs_seen = senseObstacles(curr_pos, Xobs);
|
|
||||||
centroids = [];
|
|
||||||
for i = 1:size(Xobs_seen,2)
|
|
||||||
centroids = [centroids; mean(Xobs_seen{1, i})];
|
|
||||||
end
|
|
||||||
|
|
||||||
dt = 0.01;
|
|
||||||
g = []; dg = [];
|
|
||||||
|
|
||||||
%radius size
|
|
||||||
r = 3;
|
|
||||||
|
|
||||||
for i = 1:nsteps
|
|
||||||
zInd_x = 6*(i-1) + 1;
|
|
||||||
zInd_y = 6*(i-1) + 3;
|
|
||||||
curr_xy = [z(zInd_x), z(zInd_y)];
|
|
||||||
|
|
||||||
%g based on if there's obstacles around
|
|
||||||
%initialize to zero
|
|
||||||
g_curr = 0;
|
|
||||||
zeroRow = zeros(1,size(z,2));
|
|
||||||
dg(i,:) = zeroRow;
|
|
||||||
if (~isempty(centroids))
|
|
||||||
dist2Obst = [];
|
|
||||||
for j = 1:size(Xobs_seen,2)
|
|
||||||
dist = norm(curr_xy(1) - centroids(j,1)) + norm(curr_xy(2) - centroids(j,2));
|
|
||||||
dist2Obst = [dist2Obst; dist];
|
|
||||||
end
|
|
||||||
|
|
||||||
%closest obstacle used for constraint
|
|
||||||
[minDist, indMin] = min(dist2Obst);
|
|
||||||
|
|
||||||
%g
|
|
||||||
g_curr = r^2 - (curr_xy(1) - centroids(indMin, 1))^2 - (curr_xy(2) - centroids(indMin, 2))^2;
|
|
||||||
|
|
||||||
%dg
|
|
||||||
dg(i,zInd_x) = curr_xy(1)*(-2) - centroids(indMin, 1)*2;
|
|
||||||
dg(i,zInd_y) = curr_xy(2)*(-2) - centroids(indMin, 2)*2;
|
|
||||||
end
|
|
||||||
g = [g; g_curr];
|
|
||||||
end
|
|
||||||
dg = transpose(dg);
|
|
||||||
|
|
||||||
h = z(1:6);
|
|
||||||
for i = 2:nsteps
|
|
||||||
zInd_x = 6*(i-1) + 1;
|
|
||||||
zInd_x_prev = 6*(i-2) + 1;
|
|
||||||
|
|
||||||
stateCurr = z(zInd_x:zInd_x+5);
|
|
||||||
statePrev = z(zInd_x_prev:zInd_x_prev+5);
|
|
||||||
|
|
||||||
%get previous input
|
|
||||||
uInd_prev = 2*(i-1) + nsteps*6 - 1;
|
|
||||||
uPrev = [z(uInd_prev), z(uInd_prev + 1)];
|
|
||||||
|
|
||||||
derPrev= dt*bike(statePrev, uPrev);
|
|
||||||
|
|
||||||
currH = stateCurr - statePrev - derPrev;
|
|
||||||
h(6*(i-1)+1) = currH(1);
|
|
||||||
h(6*(i-1)+2) = currH(2);
|
|
||||||
h(6*(i-1)+3) = currH(3);
|
|
||||||
h(6*(i-1)+4) = currH(4);
|
|
||||||
h(6*(i-1)+5) = currH(5);
|
|
||||||
h(6*(i-1)+6) = currH(6);
|
|
||||||
end
|
|
||||||
|
|
||||||
dh = zeros(size(z,2), nsteps*6);
|
|
||||||
dh(1:6, 1:6) = eye(6);
|
|
||||||
for i = 1:nsteps-1
|
|
||||||
|
|
||||||
|
|
||||||
end
|
|
||||||
|
|
||||||
function dzdt=bike(x,U)
|
|
||||||
%constants
|
|
||||||
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;
|
|
||||||
|
|
||||||
|
|
||||||
%generate input functions
|
|
||||||
delta_f= U(1);
|
|
||||||
F_x= U(2);
|
|
||||||
|
|
||||||
%slip angle functions in degrees
|
|
||||||
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
|
||||||
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
|
||||||
|
|
||||||
%Nonlinear Tire Dynamics
|
|
||||||
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
|
||||||
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
|
||||||
|
|
||||||
F_zf=b/(a+b)*m*g;
|
|
||||||
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
|
||||||
|
|
||||||
F_zr=a/(a+b)*m*g;
|
|
||||||
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
|
||||||
|
|
||||||
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
|
||||||
F_max=0.7*m*g;
|
|
||||||
|
|
||||||
if F_total>F_max
|
|
||||||
|
|
||||||
F_x=F_max/F_total*F_x;
|
|
||||||
|
|
||||||
F_yr=F_max/F_total*F_yr;
|
|
||||||
end
|
|
||||||
|
|
||||||
%vehicle dynamics
|
|
||||||
dzdt= [x(2)*cos(x(5))-x(4)*sin(x(5));...
|
|
||||||
(-f*m*g+Nw*F_x-F_yf*sin(delta_f))/m+x(4)*x(6);...
|
|
||||||
x(2)*sin(x(5))+x(4)*cos(x(5));...
|
|
||||||
(F_yf*cos(delta_f)+F_yr)/m-x(2)*x(6);...
|
|
||||||
x(6);...
|
|
||||||
(F_yf*a*cos(delta_f)-F_yr*b)/Iz];
|
|
||||||
end
|
|
||||||
|
|
||||||
function [A, B] =bikeLinearize(x,U)
|
|
||||||
%constants
|
|
||||||
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;
|
|
||||||
|
|
||||||
|
|
||||||
%generate input functions
|
|
||||||
delta_f= U(1);
|
|
||||||
F_x= U(2);
|
|
||||||
|
|
||||||
%slip angle functions in degrees
|
|
||||||
a_f=rad2deg(delta_f-atan2(x(4)+a*x(6),x(2)));
|
|
||||||
a_r=rad2deg(-atan2((x(4)-b*x(6)),x(2)));
|
|
||||||
|
|
||||||
%Nonlinear Tire Dynamics
|
|
||||||
phi_yf=(1-Ey)*(a_f+Shy)+(Ey/By)*atan(By*(a_f+Shy));
|
|
||||||
phi_yr=(1-Ey)*(a_r+Shy)+(Ey/By)*atan(By*(a_r+Shy));
|
|
||||||
|
|
||||||
F_zf=b/(a+b)*m*g;
|
|
||||||
F_yf=F_zf*Dy*sin(Cy*atan(By*phi_yf))+Svy;
|
|
||||||
|
|
||||||
F_zr=a/(a+b)*m*g;
|
|
||||||
F_yr=F_zr*Dy*sin(Cy*atan(By*phi_yr))+Svy;
|
|
||||||
|
|
||||||
F_total=sqrt((Nw*F_x)^2+(F_yr^2));
|
|
||||||
F_max=0.7*m*g;
|
|
||||||
|
|
||||||
if F_total>F_max
|
|
||||||
|
|
||||||
F_x=F_max/F_total*F_x;
|
|
||||||
|
|
||||||
F_yr=F_max/F_total*F_yr;
|
|
||||||
end
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
%we are just going to use cornering stiffness to make linear so this derivative
|
|
||||||
%easier, the vehicle parameter's are close enough to problem 1 hw 2
|
|
||||||
B=10;
|
|
||||||
C=1.3;
|
|
||||||
D=1;
|
|
||||||
Ca_r= Fz_r*B*C*D;
|
|
||||||
Ca_f= Fz_f*B*C*D;
|
|
||||||
|
|
||||||
A = @(t)[0, cos(x(5)), 0, -sin(x(5)), x(2)*sin(x(5))-x(4)*cos(x(5)), 0;
|
|
||||||
0, (-1/m)*Ca_f*x(2)^-2, 0, -Ca_f/m + 1, 0, Ca_f*(-a/m) + 1;
|
|
||||||
0, sin(x(5)), 0, cos(x(5)), -x(4)*sin(x(5))+x(2)*cos(x(5)), 0;
|
|
||||||
0, (1/m)*(-Ca_f*x(2)^-2 - Ca_r*x(2)^-2) - 1, 0, Ca_r/m*(-1/x(2)) + Ca_f/m*(-1/x(2)), 0, Ca_r/m*(b/x(2)) + Ca_f/m*(-a/x(2)) - u(2);
|
|
||||||
0, 0, 0, 0, 0, 1
|
|
||||||
0, (1/Iz)*(-Ca_f*a*x(2)^-2 - b*Ca_r*x(2)^-2), 0, -b*Ca_r/Iz*(-1/x(2)) + a*Ca_f/Iz*(-1/x(2)), 0, -b*Ca_r/Iz*(b/(x2)) + a*Ca_f/Iz*(-a/x(2))];
|
|
||||||
|
|
||||||
B = [0; -Ca_f/(x(2)*m); 0; Ca_f/m; 0; a*Ca_f/Iz;
|
|
||||||
0; Nw/m; 0; 0; 0; 0];
|
|
||||||
end
|
|
||||||
Reference in New Issue
Block a user