From 5233a9ef2eb377bef0db7c421be9cf6e9b3257fe Mon Sep 17 00:00:00 2001 From: Anson Date: Sun, 12 Sep 2021 18:14:20 -0700 Subject: [PATCH] update include structure --- .vscode/tasks.json | 10 +- include/sim.h | 429 +++++++++++++++++++++++++++++++++++++++++++- src/main.cpp | 6 +- src/sim.cpp | 432 --------------------------------------------- 4 files changed, 437 insertions(+), 440 deletions(-) delete mode 100644 src/sim.cpp diff --git a/.vscode/tasks.json b/.vscode/tasks.json index e4bb9db..3f3f809 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -4,10 +4,10 @@ { "type": "cppbuild", "label": "Debug", - "command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe", + "command": "g++", "args": [ "-g", - "${workspaceFolder}\\src\\*.cpp", + "${workspaceFolder}\\src\\main.cpp", "-o", "${workspaceFolder}\\build\\debug\\${fileBasenameNoExtension}.exe", "-I", @@ -23,14 +23,13 @@ "kind": "build", "isDefault": true }, - "detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\"" }, { "type": "cppbuild", "label": "Release", - "command": "C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe", + "command": "g++", "args": [ - "${workspaceFolder}\\src\\*.cpp", + "${workspaceFolder}\\src\\main.cpp", "-o", "${workspaceFolder}\\build\\release\\${fileBasenameNoExtension}.exe", "-I", @@ -46,7 +45,6 @@ "kind": "build", "isDefault": true }, - "detail": "compiler: \"C:\\Program Files\\mingw-w64\\x86_64-8.1.0-posix-seh-rt_v6-rev0\\mingw64\\bin\\g++.exe\"" } ] } \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index 2c1f6a6..bb58505 100644 --- a/include/sim.h +++ b/include/sim.h @@ -7,4 +7,431 @@ void TVC(struct sVars &, double g); void vehicleDynamics(struct sVars &, int t); void write2CSV(struct sVars &, std::fstream &outfile, int t); double derivative(double x2, double x1, double dt); -double integral(double x2, double x1, double dt); \ No newline at end of file +double integral(double x2, double x1, double dt); + +void sim(struct sVars &Vars) { + double g = -9.81; + + // defining a few random values here cause I'm lazy + Vars.burnElapsed = 2000; + Vars.m = Vars.m0; + Vars.thrust_prev = 0; + + burnStartTimeCalc(Vars, g); + + // Deleting any previous output file + if (remove("simOut.csv") != 0) + perror("Error deleting file"); + else + puts("File successfully deleted"); + + // Define and open output file "simOut.csv" + std::fstream outfile; + outfile.open("simOut.csv", std::ios::app); + + // Output file header. These are the variables that we output - useful for + // debugging + outfile + << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, " + "pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, " + "I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, " + "LQRx, LQRy" + << std::endl; + + // Start Sim + for (int t = 0; t < Vars.simTime; t++) { + thrustSelection(Vars, t); + lqrCalc(Vars); + TVC(Vars, g); + vehicleDynamics(Vars, t); + write2CSV(Vars, outfile, t); + } + + outfile.close(); +} + +void burnStartTimeCalc(struct sVars &Vars, double g) { + double v = Vars.vz; + double h = 0; + double dt = 0.001; + double a, j, m, thrust; + + for (double i = 0.148; i < 3.450; i = i + dt) { + m = Vars.m0 - i * Vars.mdot; + + if ((i > 0.147) & (i < 0.420)) + thrust = 65.165 * i - 2.3921; + + else if ((i > 0.419) & (i < 3.383)) + thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) - + 162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17; + + else if ((i > 3.382) & (i < 3.46)) + thrust = -195.78 * i + 675.11; + + v = (((thrust / m) + g) * dt) + v; + h = v * dt + h; + } + + Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height + Vars.vb = v; // terminal velocity + + double burnStartTime = Vars.vb / -g; + Vars.simTime = (Vars.tb + burnStartTime) * 1000; +} + +void thrustSelection(struct sVars &Vars, int t) { + double tol = 0.001; // 0.001 seems to be a nice tolerance + + // Check to see if current velocity is close to the F15's total velocity + bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) & + (Vars.vb > (1 - tol) * Vars.vz * -1); + + if (Vars.burnElapsed != 2000) { + // determine where in the thrust curve we're at based on elapsed burn time + // as well as current mass + Vars.burnElapsed = (t - Vars.burnStart) / 1000; + Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed); + } + + else if (b_burnStart) { + // Start burn + Vars.burnStart = t; + Vars.burnElapsed = 0; + } + + else + Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn + + if ((Vars.burnElapsed > 0.147) & (Vars.burnElapsed < 0.420)) + Vars.thrust = 65.165 * Vars.burnElapsed - 2.3921; + + else if ((Vars.burnElapsed > 0.419) & (Vars.burnElapsed < 3.383)) + Vars.thrust = + 0.8932 * pow(Vars.burnElapsed, 6) - 11.609 * pow(Vars.burnElapsed, 5) + + 60.739 * pow(Vars.burnElapsed, 4) - 162.99 * pow(Vars.burnElapsed, 3) + + 235.6 * pow(Vars.burnElapsed, 2) - 174.43 * Vars.burnElapsed + 67.17; + + else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46)) + Vars.thrust = -195.78 * Vars.burnElapsed + 675.11; +} + +void lqrCalc(struct sVars &Vars) { + + Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + + pow(Vars.vehicleRadius, 2) / 4); + Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + + pow(Vars.vehicleRadius, 2) / 4); + Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2); + + /* + + double n = sqrt(398600 / pow(6678, 3)); + double k1 = (Vars.I22 - Vars.I33) / Vars.I11; + double k2 = (Vars.I11 - Vars.I33) / Vars.I22; + double k3 = (Vars.I22 - Vars.I11) / Vars.I33; + + double R11 = pow(10, 2); + double R22 = pow(10, 2); + double R33 = pow(10, 2); + + double F11 = -2 * pow(n, 2) * 4 * k1; + double F22 = -2 * pow(n, 2) * 3 * k2; + double F33 = -2 * pow(n, 2) * k3; + + double G31 = n * (1 - k1); + double G13 = n * (k3 - 1); + + double d = 0.5; + /* + The following calculations are based on output of LQR.m to avoid working + with matrices in C++ Please see .m file for details on calculation. + Algorithm taken from LQR wikipedia page: + https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution +*/ + /* + double gain = 0.25; + + double K11 = + -1 * gain * + (Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + + R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) / + (R11 * d * Vars.I11 * + (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); + double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22); + double K33 = + -1 * gain * + (Vars.I33 * + (Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + + R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) / + (R33 * d * pow(abs(Vars.I33), 2) * + (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); + double K34 = + gain * (R11 * pow(abs(Vars.I11), 2) * G13) / + (R33 * Vars.I33 * + (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); + double K16 = + gain * (R33 * pow(abs(Vars.I33), 2) * G31) / + (R11 * Vars.I11 * + (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); + + // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm + Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm; + Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm; + + */ + + // Paste in Values from gainCalc.m + double K11 = 39.54316; + double K12 = 0.00000; + double K13 = -0.00000; + double K14 = 39.55769; + double K15 = 0.00000; + double K16 = 0.00000; + double K21 = 0.00000; + double K22 = 39.54316; + double K23 = 0.00000; + double K24 = 0.00000; + double K25 = 39.55769; + double K26 = 0.00000; + double K31 = 0.00000; + double K32 = 0.00000; + double K33 = 39.54316; + double K34 = 0.00000; + double K35 = 0.00000; + double K36 = 39.54394; + + double gain = + 0.25 * + pow(10, -4); // changing exponenet drastically changes results of LQR + + // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm + Vars.LQRx = + gain * + ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + + K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / + -Vars.momentArm; + Vars.LQRy = + gain * + ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 + + K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) / + -Vars.momentArm; + + // LQR Force limiter X + if (Vars.LQRx > Vars.thrust) + Vars.LQRx = Vars.thrust; + else if (Vars.LQRx < -1 * Vars.thrust) + Vars.LQRx = -1 * Vars.thrust; + + // LQR Force limiter Y + if (Vars.LQRy > Vars.thrust) + Vars.LQRy = Vars.thrust; + else if (Vars.LQRy < -1 * Vars.thrust) + Vars.LQRy = -1 * Vars.thrust; +} + +void TVC(struct sVars &Vars, double g) { + if (Vars.thrust < 1) { + // Define forces and moments for t = 0 + Vars.Fx = 0; + Vars.Fy = 0; + Vars.Fz = g * Vars.m0; + + Vars.momentX = 0; + Vars.momentY = 0; + Vars.momentZ = 0; + } + + else { + // Convert servo position to degrees for comparison to max allowable + Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust); + + // Servo position limiter + if (Vars.xServoDegs > Vars.maxServo) + Vars.xServoDegs = Vars.maxServo; + else if (Vars.xServoDegs < -1 * Vars.maxServo) + Vars.xServoDegs = -1 * Vars.maxServo; + + // Convert servo position to degrees for comparison to max allowable + Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.thrust); + + // Servo position limiter + if (Vars.yServoDegs > Vars.maxServo) + Vars.yServoDegs = Vars.maxServo; + else if (Vars.yServoDegs < -1 * Vars.maxServo) + Vars.yServoDegs = -1 * Vars.maxServo; + + // Vector math to aqcuire thrust vector components + Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180)); + Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180)); + Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) + + (Vars.m * g); + + // Calculate moment created by Fx and Fy + Vars.momentX = Vars.Fx * Vars.momentArm; + Vars.momentY = Vars.Fy * Vars.momentArm; + Vars.momentZ = 0; + } +} + +void vehicleDynamics(struct sVars &Vars, int t) { + // Idot + if (t < 1) { + Vars.I11dot = 0; + Vars.I22dot = 0; + Vars.I33dot = 0; + } else { + Vars.I11dot = derivative(Vars.I11, Vars.I11prev, Vars.stepSize); + Vars.I22dot = derivative(Vars.I22, Vars.I22prev, Vars.stepSize); + Vars.I33dot = derivative(Vars.I33, Vars.I33prev, Vars.stepSize); + } + + // pdot, qdot, rdot + Vars.yawddot = (Vars.momentX - Vars.I11dot * Vars.yawdot + + Vars.I22 * Vars.pitchdot * Vars.rolldot - + Vars.I33 * Vars.pitchdot * Vars.rolldot) / + Vars.I11; + Vars.pitchddot = (Vars.momentY - Vars.I22dot * Vars.pitchdot - + Vars.I11 * Vars.rolldot * Vars.yawdot + + Vars.I33 * Vars.rolldot * Vars.yawdot) / + Vars.I22; + Vars.rollddot = (Vars.momentZ - Vars.I33dot * Vars.rolldot + + Vars.I11 * Vars.pitchdot * Vars.yawdot - + Vars.I22 * Vars.pitchdot * Vars.yawdot) / + Vars.I33; + + if (t < 1) { + Vars.x = 0; + Vars.y = 0; + + Vars.ax = 0; + Vars.ay = 0; + Vars.az = Vars.Fz / Vars.m0; + } + + else { + // p, q, r + Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize); + Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize); + Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize); + + // ax ay az + Vars.ax = + (Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy); + Vars.ay = + (Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot); + Vars.az = + (Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx); + + // vx vy vz in Body frame + Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize); + Vars.vy = integral(Vars.ay, Vars.vyPrev, Vars.stepSize); + Vars.vz = integral(Vars.az, Vars.vzPrev, Vars.stepSize); + + // Xe + Vars.x = integral(Vars.vx, Vars.xPrev, Vars.stepSize); + Vars.y = integral(Vars.vy, Vars.yPrev, Vars.stepSize); + Vars.z = integral(Vars.vz, Vars.zPrev, Vars.stepSize); + + // Euler Angles + Vars.phidot = Vars.yawdot + (Vars.pitchdot * sin(Vars.yaw) + + Vars.rolldot * cos(Vars.yaw)) * + (sin(Vars.pitch) / cos(Vars.pitch)); + Vars.thetadot = + Vars.pitchdot * cos(Vars.yaw) - Vars.rolldot * sin(Vars.pitch); + Vars.psidot = + (Vars.pitchdot * sin(Vars.yaw) + Vars.rolldot * cos(Vars.yaw)) / + cos(Vars.pitch); + + Vars.yaw = integral(Vars.phidot, Vars.yawPrev, Vars.stepSize); + Vars.pitch = integral(Vars.thetadot, Vars.pitchPrev, Vars.stepSize); + Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize); + } + + // Set "prev" values for next timestep + Vars.I11prev = Vars.I11; + Vars.I22prev = Vars.I22; + Vars.I33prev = Vars.I33; + + Vars.yawPrev = Vars.yaw; + Vars.pitchPrev = Vars.pitch; + Vars.rollPrev = Vars.roll; + + Vars.yawdotPrev = Vars.yawdot; + Vars.pitchdotPrev = Vars.pitchdot; + Vars.rolldotPrev = Vars.rolldot; + + Vars.yawddotPrev = Vars.yawddot; + Vars.pitchddotPrev = Vars.pitchddot; + Vars.rollddotPrev = Vars.rollddot; + + Vars.axPrev = Vars.ax; + Vars.ayPrev = Vars.ay; + Vars.azPrev = Vars.az; + + Vars.vxPrev = Vars.vx; + Vars.vyPrev = Vars.vy; + Vars.vzPrev = Vars.vz; + + Vars.xPrev = Vars.x; + Vars.yPrev = Vars.y; + Vars.zPrev = Vars.z; +} + +void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) { + // writing to output file + outfile << t << ", "; + + outfile << Vars.x << ", "; + outfile << Vars.y << ", "; + outfile << Vars.z << ", "; + + outfile << Vars.vx << ", "; + outfile << Vars.vy << ", "; + outfile << Vars.vz << ", "; + + outfile << Vars.ax << ", "; + outfile << Vars.ay << ", "; + outfile << Vars.az << ", "; + + outfile << Vars.yaw * 180 / 3.1416 << ", "; + outfile << Vars.pitch * 180 / 3.1416 << ", "; + outfile << Vars.roll * 180 / 3.1416 << ", "; + + outfile << Vars.yawdot * 180 / 3.1416 << ", "; + outfile << Vars.pitchdot * 180 / 3.1416 << ", "; + outfile << Vars.rolldot * 180 / 3.1416 << ", "; + + outfile << Vars.yawddot * 180 / 3.1416 << ", "; + outfile << Vars.pitchddot * 180 / 3.1416 << ", "; + outfile << Vars.rollddot * 180 / 3.1416 << ", "; + + outfile << Vars.I11 << ", "; + outfile << Vars.I22 << ", "; + outfile << Vars.I33 << ", "; + + outfile << Vars.I11dot << ", "; + outfile << Vars.I22dot << ", "; + outfile << Vars.I33dot << ", "; + + outfile << Vars.xServoDegs << ", "; + outfile << Vars.yServoDegs << ", "; + + outfile << Vars.m << ", "; + outfile << Vars.thrust << ", "; + outfile << Vars.burnElapsed << ", "; + outfile << Vars.Fz << ", "; + + outfile << Vars.LQRx << ", "; + outfile << Vars.LQRy << std::endl; +} + +double derivative(double x2, double x1, double dt) { + double dxdt = (x2 - x1) / (dt / 1000); + return dxdt; +} + +double integral(double x, double y, double dt) { + double integ = (x * dt / 1000) + y; + return integ; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 3dbaae0..23b5f31 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,6 +1,10 @@ +#include +#include +#include +#include + #include "sVars.h" #include "sim.h" -#include void sim(struct sVars &); diff --git a/src/sim.cpp b/src/sim.cpp deleted file mode 100644 index b3920ed..0000000 --- a/src/sim.cpp +++ /dev/null @@ -1,432 +0,0 @@ -#include "sim.h" -#include -#include -#include -#include - -void sim(struct sVars &Vars) { - double g = -9.81; - - // defining a few random values here cause I'm lazy - Vars.burnElapsed = 2000; - Vars.m = Vars.m0; - Vars.thrust_prev = 0; - - burnStartTimeCalc(Vars, g); - - // Deleting any previous output file - if (remove("simOut.csv") != 0) - perror("Error deleting file"); - else - puts("File successfully deleted"); - - // Define and open output file "simOut.csv" - std::fstream outfile; - outfile.open("simOut.csv", std::ios::app); - - // Output file header. These are the variables that we output - useful for - // debugging - outfile - << "t, x, y, z, vx, vy, vz, ax, ay, az, yaw, pitch, roll, yawdot, " - "pitchdot, rolldot, yawddot, pitchddot, rollddot, I11, I22, I33, " - "I11dot, I22dot, I33dot, Servo1, Servo2, m, thrust, burnElapsed, Fz, " - "LQRx, LQRy" - << std::endl; - - // Start Sim - for (int t = 0; t < Vars.simTime; t++) { - thrustSelection(Vars, t); - lqrCalc(Vars); - TVC(Vars, g); - vehicleDynamics(Vars, t); - write2CSV(Vars, outfile, t); - } - - outfile.close(); -} - -void burnStartTimeCalc(struct sVars &Vars, double g) { - double v = Vars.vz; - double h = 0; - double dt = 0.001; - double a, j, m, thrust; - - for (double i = 0.148; i < 3.450; i = i + dt) { - m = Vars.m0 - i * Vars.mdot; - - if ((i > 0.147) & (i < 0.420)) - thrust = 65.165 * i - 2.3921; - - else if ((i > 0.419) & (i < 3.383)) - thrust = 0.8932 * pow(i, 6) - 11.609 * pow(i, 5) + 60.739 * pow(i, 4) - - 162.99 * pow(i, 3) + 235.6 * pow(i, 2) - 174.43 * i + 67.17; - - else if ((i > 3.382) & (i < 3.46)) - thrust = -195.78 * i + 675.11; - - v = (((thrust / m) + g) * dt) + v; - h = v * dt + h; - } - - Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height - Vars.vb = v; // terminal velocity - - double burnStartTime = Vars.vb / -g; - Vars.simTime = (Vars.tb + burnStartTime) * 1000; -} - -void thrustSelection(struct sVars &Vars, int t) { - double tol = 0.001; // 0.001 seems to be a nice tolerance - - // Check to see if current velocity is close to the F15's total velocity - bool b_burnStart = (Vars.vb < (1 + tol) * Vars.vz * -1) & - (Vars.vb > (1 - tol) * Vars.vz * -1); - - if (Vars.burnElapsed != 2000) { - // determine where in the thrust curve we're at based on elapsed burn time - // as well as current mass - Vars.burnElapsed = (t - Vars.burnStart) / 1000; - Vars.m = Vars.m0 - (Vars.mdot * Vars.burnElapsed); - } - - else if (b_burnStart) { - // Start burn - Vars.burnStart = t; - Vars.burnElapsed = 0; - } - - else - Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn - - if ((Vars.burnElapsed > 0.147) & (Vars.burnElapsed < 0.420)) - Vars.thrust = 65.165 * Vars.burnElapsed - 2.3921; - - else if ((Vars.burnElapsed > 0.419) & (Vars.burnElapsed < 3.383)) - Vars.thrust = - 0.8932 * pow(Vars.burnElapsed, 6) - 11.609 * pow(Vars.burnElapsed, 5) + - 60.739 * pow(Vars.burnElapsed, 4) - 162.99 * pow(Vars.burnElapsed, 3) + - 235.6 * pow(Vars.burnElapsed, 2) - 174.43 * Vars.burnElapsed + 67.17; - - else if ((Vars.burnElapsed > 3.382) & (Vars.burnElapsed < 3.46)) - Vars.thrust = -195.78 * Vars.burnElapsed + 675.11; -} - -void lqrCalc(struct sVars &Vars) { - - Vars.I11 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + - pow(Vars.vehicleRadius, 2) / 4); - Vars.I22 = Vars.m * ((1 / 12) * pow(Vars.vehicleHeight, 2) + - pow(Vars.vehicleRadius, 2) / 4); - Vars.I33 = Vars.m * 0.5 * pow(Vars.vehicleRadius, 2); - - /* - - double n = sqrt(398600 / pow(6678, 3)); - double k1 = (Vars.I22 - Vars.I33) / Vars.I11; - double k2 = (Vars.I11 - Vars.I33) / Vars.I22; - double k3 = (Vars.I22 - Vars.I11) / Vars.I33; - - double R11 = pow(10, 2); - double R22 = pow(10, 2); - double R33 = pow(10, 2); - - double F11 = -2 * pow(n, 2) * 4 * k1; - double F22 = -2 * pow(n, 2) * 3 * k2; - double F33 = -2 * pow(n, 2) * k3; - - double G31 = n * (1 - k1); - double G13 = n * (k3 - 1); - - double d = 0.5; - /* - The following calculations are based on output of LQR.m to avoid working - with matrices in C++ Please see .m file for details on calculation. - Algorithm taken from LQR wikipedia page: - https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution -*/ - /* - double gain = 0.25; - - double K11 = - -1 * gain * - (Vars.I33 * pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + - R33 * pow(abs(G31), 2) * pow(abs(Vars.I33), 2) + Vars.I33) / - (R11 * d * Vars.I11 * - (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); - double K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22); - double K33 = - -1 * gain * - (Vars.I33 * - (Vars.I33 * pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + - R11 * pow(abs(G13), 2) * pow(abs(Vars.I11), 2) + Vars.I33)) / - (R33 * d * pow(abs(Vars.I33), 2) * - (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); - double K34 = - gain * (R11 * pow(abs(Vars.I11), 2) * G13) / - (R33 * Vars.I33 * - (pow(abs(F11), 2) * pow(abs(Vars.I11), 4) * pow(abs(R11), 2) + 1)); - double K16 = - gain * (R33 * pow(abs(Vars.I33), 2) * G31) / - (R11 * Vars.I11 * - (pow(abs(F33), 2) * pow(abs(Vars.I33), 4) * pow(abs(R33), 2) + 1)); - - // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm - Vars.LQRx = (K11 * Vars.yaw / 2 + K16 * Vars.rolldot) / Vars.momentArm; - Vars.LQRy = (K22 * Vars.pitch / 2) / Vars.momentArm; - - */ - - // Paste in Values from gainCalc.m - double K11 = 39.54316; - double K12 = 0.00000; - double K13 = -0.00000; - double K14 = 39.55769; - double K15 = 0.00000; - double K16 = 0.00000; - double K21 = 0.00000; - double K22 = 39.54316; - double K23 = 0.00000; - double K24 = 0.00000; - double K25 = 39.55769; - double K26 = 0.00000; - double K31 = 0.00000; - double K32 = 0.00000; - double K33 = 39.54316; - double K34 = 0.00000; - double K35 = 0.00000; - double K36 = 39.54394; - - double gain = - 0.25 * - pow(10, -4); // changing exponenet drastically changes results of LQR - - // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm - Vars.LQRx = - gain * - ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + - K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / - -Vars.momentArm; - Vars.LQRy = - gain * - ((K22 * Vars.pitch) / 2 + K25 * Vars.pitchdot + (K23 * Vars.roll) / 2 + - K26 * Vars.rolldot + (K21 * Vars.yaw) / 2 + K24 * Vars.yawdot) / - -Vars.momentArm; - - // LQR Force limiter X - if (Vars.LQRx > Vars.thrust) - Vars.LQRx = Vars.thrust; - else if (Vars.LQRx < -1 * Vars.thrust) - Vars.LQRx = -1 * Vars.thrust; - - // LQR Force limiter Y - if (Vars.LQRy > Vars.thrust) - Vars.LQRy = Vars.thrust; - else if (Vars.LQRy < -1 * Vars.thrust) - Vars.LQRy = -1 * Vars.thrust; -} - -void TVC(struct sVars &Vars, double g) { - if (Vars.thrust < 1) { - // Define forces and moments for t = 0 - Vars.Fx = 0; - Vars.Fy = 0; - Vars.Fz = g * Vars.m0; - - Vars.momentX = 0; - Vars.momentY = 0; - Vars.momentZ = 0; - } - - else { - // Convert servo position to degrees for comparison to max allowable - Vars.xServoDegs = (180 / 3.1416) * asin(Vars.LQRx / Vars.thrust); - - // Servo position limiter - if (Vars.xServoDegs > Vars.maxServo) - Vars.xServoDegs = Vars.maxServo; - else if (Vars.xServoDegs < -1 * Vars.maxServo) - Vars.xServoDegs = -1 * Vars.maxServo; - - // Convert servo position to degrees for comparison to max allowable - Vars.yServoDegs = (180 / 3.1416) * asin(Vars.LQRy / Vars.thrust); - - // Servo position limiter - if (Vars.yServoDegs > Vars.maxServo) - Vars.yServoDegs = Vars.maxServo; - else if (Vars.yServoDegs < -1 * Vars.maxServo) - Vars.yServoDegs = -1 * Vars.maxServo; - - // Vector math to aqcuire thrust vector components - Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (3.1416 / 180)); - Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (3.1416 / 180)); - Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) + - (Vars.m * g); - - // Calculate moment created by Fx and Fy - Vars.momentX = Vars.Fx * Vars.momentArm; - Vars.momentY = Vars.Fy * Vars.momentArm; - Vars.momentZ = 0; - } -} - -void vehicleDynamics(struct sVars &Vars, int t) { - // Idot - if (t < 1) { - Vars.I11dot = 0; - Vars.I22dot = 0; - Vars.I33dot = 0; - } else { - Vars.I11dot = derivative(Vars.I11, Vars.I11prev, Vars.stepSize); - Vars.I22dot = derivative(Vars.I22, Vars.I22prev, Vars.stepSize); - Vars.I33dot = derivative(Vars.I33, Vars.I33prev, Vars.stepSize); - } - - // pdot, qdot, rdot - Vars.yawddot = (Vars.momentX - Vars.I11dot * Vars.yawdot + - Vars.I22 * Vars.pitchdot * Vars.rolldot - - Vars.I33 * Vars.pitchdot * Vars.rolldot) / - Vars.I11; - Vars.pitchddot = (Vars.momentY - Vars.I22dot * Vars.pitchdot - - Vars.I11 * Vars.rolldot * Vars.yawdot + - Vars.I33 * Vars.rolldot * Vars.yawdot) / - Vars.I22; - Vars.rollddot = (Vars.momentZ - Vars.I33dot * Vars.rolldot + - Vars.I11 * Vars.pitchdot * Vars.yawdot - - Vars.I22 * Vars.pitchdot * Vars.yawdot) / - Vars.I33; - - if (t < 1) { - Vars.x = 0; - Vars.y = 0; - - Vars.ax = 0; - Vars.ay = 0; - Vars.az = Vars.Fz / Vars.m0; - } - - else { - // p, q, r - Vars.yawdot = integral(Vars.yawddot, Vars.yawdotPrev, Vars.stepSize); - Vars.pitchdot = integral(Vars.pitchddot, Vars.pitchdotPrev, Vars.stepSize); - Vars.rolldot = integral(Vars.rollddot, Vars.rolldotPrev, Vars.stepSize); - - // ax ay az - Vars.ax = - (Vars.Fx / Vars.m) + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy); - Vars.ay = - (Vars.Fy / Vars.m) + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot); - Vars.az = - (Vars.Fz / Vars.m) + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx); - - // vx vy vz in Body frame - Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize); - Vars.vy = integral(Vars.ay, Vars.vyPrev, Vars.stepSize); - Vars.vz = integral(Vars.az, Vars.vzPrev, Vars.stepSize); - - // Xe - Vars.x = integral(Vars.vx, Vars.xPrev, Vars.stepSize); - Vars.y = integral(Vars.vy, Vars.yPrev, Vars.stepSize); - Vars.z = integral(Vars.vz, Vars.zPrev, Vars.stepSize); - - // Euler Angles - Vars.phidot = Vars.yawdot + (Vars.pitchdot * sin(Vars.yaw) + - Vars.rolldot * cos(Vars.yaw)) * - (sin(Vars.pitch) / cos(Vars.pitch)); - Vars.thetadot = - Vars.pitchdot * cos(Vars.yaw) - Vars.rolldot * sin(Vars.pitch); - Vars.psidot = - (Vars.pitchdot * sin(Vars.yaw) + Vars.rolldot * cos(Vars.yaw)) / - cos(Vars.pitch); - - Vars.yaw = integral(Vars.phidot, Vars.yawPrev, Vars.stepSize); - Vars.pitch = integral(Vars.thetadot, Vars.pitchPrev, Vars.stepSize); - Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize); - } - - // Set "prev" values for next timestep - Vars.I11prev = Vars.I11; - Vars.I22prev = Vars.I22; - Vars.I33prev = Vars.I33; - - Vars.yawPrev = Vars.yaw; - Vars.pitchPrev = Vars.pitch; - Vars.rollPrev = Vars.roll; - - Vars.yawdotPrev = Vars.yawdot; - Vars.pitchdotPrev = Vars.pitchdot; - Vars.rolldotPrev = Vars.rolldot; - - Vars.yawddotPrev = Vars.yawddot; - Vars.pitchddotPrev = Vars.pitchddot; - Vars.rollddotPrev = Vars.rollddot; - - Vars.axPrev = Vars.ax; - Vars.ayPrev = Vars.ay; - Vars.azPrev = Vars.az; - - Vars.vxPrev = Vars.vx; - Vars.vyPrev = Vars.vy; - Vars.vzPrev = Vars.vz; - - Vars.xPrev = Vars.x; - Vars.yPrev = Vars.y; - Vars.zPrev = Vars.z; -} - -void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) { - // writing to output file - outfile << t << ", "; - - outfile << Vars.x << ", "; - outfile << Vars.y << ", "; - outfile << Vars.z << ", "; - - outfile << Vars.vx << ", "; - outfile << Vars.vy << ", "; - outfile << Vars.vz << ", "; - - outfile << Vars.ax << ", "; - outfile << Vars.ay << ", "; - outfile << Vars.az << ", "; - - outfile << Vars.yaw * 180 / 3.1416 << ", "; - outfile << Vars.pitch * 180 / 3.1416 << ", "; - outfile << Vars.roll * 180 / 3.1416 << ", "; - - outfile << Vars.yawdot * 180 / 3.1416 << ", "; - outfile << Vars.pitchdot * 180 / 3.1416 << ", "; - outfile << Vars.rolldot * 180 / 3.1416 << ", "; - - outfile << Vars.yawddot * 180 / 3.1416 << ", "; - outfile << Vars.pitchddot * 180 / 3.1416 << ", "; - outfile << Vars.rollddot * 180 / 3.1416 << ", "; - - outfile << Vars.I11 << ", "; - outfile << Vars.I22 << ", "; - outfile << Vars.I33 << ", "; - - outfile << Vars.I11dot << ", "; - outfile << Vars.I22dot << ", "; - outfile << Vars.I33dot << ", "; - - outfile << Vars.xServoDegs << ", "; - outfile << Vars.yServoDegs << ", "; - - outfile << Vars.m << ", "; - outfile << Vars.thrust << ", "; - outfile << Vars.burnElapsed << ", "; - outfile << Vars.Fz << ", "; - - outfile << Vars.LQRx << ", "; - outfile << Vars.LQRy << std::endl; -} - -double derivative(double x2, double x1, double dt) { - double dxdt = (x2 - x1) / (dt / 1000); - return dxdt; -} - -double integral(double x, double y, double dt) { - double integ = (x * dt / 1000) + y; - return integ; -} \ No newline at end of file