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

past attempts/diff calculations

Browse Source
This commit is contained in:
xdemenchuk
2021-12-06 12:54:58 -05:00
parent 82acad1e04
commit 877912e9bf
1 changed files with 61 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

74
SimPkg_F21(student_ver)/xenia_attempts_save.m
View File

@@ -1,9 +1,9 @@
%% trying to figure out jacobian
%constants
syms Nw f Iz a b By Cy Dy Ey Shy Svy m g
syms Nw f Iz a b By Cy Dy Ey Shy Svy m g real
syms delta_f F_x
x = sym('x', [1 6]);
syms delta_f F_x real
x = sym('x', [1 6], 'real');
%generate input functions
% delta_f=interp1(T,U(:,1),t,'previous','extrap');
% F_x=interp1(T,U(:,2),t,'previous','extrap');
@@ -26,21 +26,69 @@ F_total=sqrt((Nw*F_x)^2+(F_yr^2));
F_max=0.7*m*g;
% eq1 = x(2)*cos(x(5))-x(4)*sin(x(5));
% a11 = diff(eq1, x(1))
% a12 = diff(eq1, x(2))
% a13 = diff(eq1, x(3))
% a14 = diff(eq1, x(4))
% a15 = diff(eq1, x(5))
% a16 = diff(eq1, x(6))
% b11 = diff(eq1, delta_f)
% b12 = diff(eq1, F_x)
negFyf = -F_yf;
negFyfsindeltaf = negFyf*sin(delta_f);
% eq2 = 1/m*(-f*m*g + Nw*F_x - F_yf*sin(delta_f))+x(4)*x(6);
% a21 = diff(eq2, x(1));
% a22 = diff(eq2, x(2));
% a23 = diff(eq2, x(3));
% a24 = diff(eq2, x(4));
% a25 = diff(eq2, x(5));
% a26 = diff(eq2, x(6));
% b21 = diff(eq2, delta_f);
% b22 = diff(eq2, F_x);
Fyfcosdeltaf = F_yf*cos(delta_f);
% eq3 = x(2)*sin(x(5)) + x(4)*cos(x(5));
% a31 = diff(eq3, x(1))
% a32 = diff(eq3, x(2))
% a33 = diff(eq3, x(3))
% a34 = diff(eq3, x(4))
% a35 = diff(eq3, x(5))
% a36 = diff(eq3, x(6))
% b31 = diff(eq3, delta_f)
% b32 = diff(eq3, F_x)
aFyfcosdeltaf = a*F_yf*cos(delta_f);
% eq4 = 1/m*(F_yf*cos(delta_f) + F_yr) - x(2)*x(4)
% a41 = diff(eq4, x(1))
% a42 = diff(eq4, x(2))
% a43 = diff(eq4, x(3))
% a44 = diff(eq4, x(4))
% a45 = diff(eq4, x(5))
% a46 = diff(eq4, x(6))
% b41 = diff(eq4, delta_f)
% b42 = diff(eq4, F_x)
negbFyr = -b*F_yr;
eq5 = x(6);
% a51 = diff(eq5, x(1))
% a52 = diff(eq5, x(2))
% a53 = diff(eq5, x(3))
% a54 = diff(eq5, x(4))
% a55 = diff(eq5, x(5))
% a56 = diff(eq5, x(6))
% b51 = diff(eq5, delta_f)
% b52 = diff(eq5, F_x)
eq1difu = diff(negFyfsindeltaf, x(2))
eq1difv = diff(negFyfsindeltaf, x(4))
eq1difr = diff(negFyfsindeltaf, x(6))
eq1difdeltaf = diff(negFyfsindeltaf, delta_f)
eq1difFx = diff(negFyfsindeltaf, F_x)
eq6 = 1/Iz*(a*F_yf*cos(delta_f)-b*F_yr);
a61 = diff(eq6, x(1))
a62 = diff(eq6, x(2))
a63 = diff(eq6, x(3))
a64 = diff(eq6, x(4))
a65 = diff(eq6, x(5))
a66 = diff(eq6, x(6))
b61 = diff(eq6, delta_f)
b62 = diff(eq6, F_x)
% (delta_f - arctan((v+a*r)/u))
% ((1-E_y)*((delta_f - arctan((v+a*r)/u)) + S_hy) + E_y/B_y * arctan(B_y*((delta_f - arctan((v+a*r)/u))+S_hy)))
% F_zf*D_y*sin(C_y*arctan(B_y*((1-E_y)*((delta_f - arctan((v+a*r)/u)) + S_hy) + E_y/B_y
% * arctan(B_y*((delta_f - arctan((v+a*r)/u))+S_hy))))) + S_vy
%% nonlinear using reference trajectory segments