From e83923c87602f9929643ab3b6a6afb6beb4fcdb8 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Sun, 12 Sep 2021 15:52:18 -0700 Subject: [PATCH] adding build files --- build/debug/main.exe | Bin 139557 -> 139557 bytes build/release/gainCalc.m | 66 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 66 insertions(+) create mode 100644 build/release/gainCalc.m diff --git a/build/debug/main.exe b/build/debug/main.exe index 4708f92f0d70a362778ad2d2b4792fe5d920b06e..26e4c04ff72d33b6ab0ad8ff0ff487e80c3dbbbf 100644 GIT binary patch delta 746 zcmYk&PiPZC6bA75HrwqYMGB&crI;EO#FjSMZ4{-4VnGzaLe)}wun0-&R+~s&HA^fds-SrB7zLrH;+yUSy)69Ry!XDDonO-Dm-PIs z^5s_H?g{1Kou{j(D=_E6)vYiWj#T}CK6j=T!ls3rwa$@185rzgOewI~*x1D>D>Y@= z*BHufDU5cY#J@+-6D}ceFY0yCVF@8mdMsuOc6n@_u#$FsoZ&g|H{eH+j~FnI717oh zPDdDu*z^B%5#h#kEG^b8qQxtmW9P5ZZQ{nj$zg^ZG=5uC`gp5}_FXybut&WMr7qhE z>q1;~7}qyyx>%FrgC_PO$kQecbp)}y(NAYVbbF81+2pC=0fstXG10ZT9*|85dPJSK z^kOLcLf+UhLpcq!RWG#OrP&{uetCHFGCmVP1}Wq2d~NO(WPTj>X6gMP-c^nGDk z|4uqDO#4@|vM?(O6!2d9hA;(ukY3A>Dd3}YoorG-Mf#^O1yrTK2~$8#x+YBfKS?hO z)Bez^ER;oo0zOMGW>Vv4tt7)&HxtF518C#+BrbLMe>qocFpI{ zdu6vdi6I?*?&uYCYWtj-$Bqi>UswJO_{4xSlW!c@wxew0zoxN;|Cq*3mp+Fd{sk#6 B%-sM0 delta 746 zcmYk&PiPZC6bA75HrwqAMGB(bN(nV8h%Ifh+bF1qVnGzaLe)}wuoO0FS4|>y)hKPE zDMFDT2|bKQ4;Dl`C_ThWP#UHFw^o~^#e?F-TT~Fd2)^k~(96Q_&3o^g*|`OMZb8q@ zD6x3{#xdod{c!DM8Rl%TvKi+5p-KzTID4uZz=rv2)s7)!5$Nw`Ovy8QWaQk0lbCSa z%M6Q!r$Mv9*|(c6w|+?Zn;47{fE(YoY~3K4ii=QpDE! za5BVD#LoYxiwM@I?W9=u7)@SboSnHuw~3g2#|Ig*(D-#tIl!AOv~ADA?+$wxN?dTG z&e@3QFt4oFbg?GO`z`E(pC>KsYxiSEy`Rqf=<*(|v)GBjK86}!w$Qn;X2_-}J)*{& zdN7cFA#ZayaWrHyyp-%ES`4q&>B~P#lY1CmOFs|}GrW<$E4+u{t@LfONk8MA^etgp zza*U#rv2~JvM?hG6tFCPRhR-kNdHceDd3~@cd|tRE7ISDDWELa-43aK!D?L{khV>sV#{LH9I1AOCpSD7g)w5X~!v}!&h z=ambMaSZ6#Ul_iK4sG}J)cuSK>i?ts8}NyN>8V`(z;!dq7XD)roA}pBY%9>`;GMrW CI?Q$e diff --git a/build/release/gainCalc.m b/build/release/gainCalc.m new file mode 100644 index 0000000..4301efe --- /dev/null +++ b/build/release/gainCalc.m @@ -0,0 +1,66 @@ +clear all; clc; +%% User Defined Values +M0 = 1.2; % [kg] +vehicleHeight = 0.5318; % [m] +vehicleRadius = 0.05105; % [m] + +%% Calcs +I11 = (1/12) * vehicleHeight^2 + 0.25 * vehicleRadius^2; % (1/12) * h^2 + 0.25 * r^2 +I22 = (1/12) * vehicleHeight^2 + 0.25 * vehicleRadius^2; % (1/12) * h^2 + 0.25 * r^2 +I33 = 0.5 * vehicleRadius^2; % 0.5 * r^2 +I = M0 * [I11 0 0; 0 I22 0; 0 0 I33]; + +Q = eye(6) * I(3, 3); +Mu = 398600; +r0 = [6678; 0; 0]; + +k1 = (I(2,2) - I(3,3)) / I(1,1); +k2 = (I(1,1) - I(3,3)) / I(2,2); +k3 = (I(2,2) - I(1,1)) / I(3,3); + +n = sqrt(Mu/(norm(r0)^3)); + +F = -2 * n^2 * [4 * k1, 0, 0; 0, 3 * k2, 0; 0, 0, k3]; +G = n * [0, 0, (1 - k1); 0, 0, 0; (k3 - 1), 0, 0]; + +Iinv = [1 / I(1,1), 0, 0; 0, 1 / I(2,2), 0; 0, 0, 1 / I(3,3)]; + +A = [zeros(3,3), 0.5 * eye(3); F, G]; +B = [zeros(3,3); Iinv]; + +R = eye(3)*10^-6; +Rinv = R^-1; + +S = icare(A, B, Q, R); + +K = Rinv * B' * S; + +%% Outputs +% Copy results in command window to LQRcalc function in C++ +fprintf("double K11 = %3.5f;\n", K(1, 1)) +fprintf("double K12 = %3.5f;\n", K(1, 2)) +fprintf("double K13 = %3.5f;\n", K(1, 3)) +fprintf("double K14 = %3.5f;\n", K(1, 4)) +fprintf("double K15 = %3.5f;\n", K(1, 5)) +fprintf("double K16 = %3.5f;\n", K(1, 6)) +fprintf("double K21 = %3.5f;\n", K(2, 1)) +fprintf("double K22 = %3.5f;\n", K(2, 2)) +fprintf("double K23 = %3.5f;\n", K(2, 3)) +fprintf("double K24 = %3.5f;\n", K(2, 4)) +fprintf("double K25 = %3.5f;\n", K(2, 5)) +fprintf("double K26 = %3.5f;\n", K(2, 6)) +fprintf("double K31 = %3.5f;\n", K(3, 1)) +fprintf("double K32 = %3.5f;\n", K(3, 2)) +fprintf("double K33 = %3.5f;\n", K(3, 3)) +fprintf("double K34 = %3.5f;\n", K(3, 4)) +fprintf("double K35 = %3.5f;\n", K(3, 5)) +fprintf("double K36 = %3.5f;\n", K(3, 6)) + +syms yaw pitch roll yawdot pitchdot rolldot gain +w = [0.5*yaw 0.5*pitch 0.5*roll yawdot pitchdot rolldot]'; + +syms K11 K12 K13 K14 K15 K16 K21 K22 K23 K24 K25 K26 K31 K32 K33 K34 K35 K36 +K = gain * [K11 K12 K13 K14 K15 K16; K21 K22 K23 K24 K25 K26; K31 K32 K33 K34 K35 K36]; + +simplify(K*w) +