From 9f5cd7bdf75c3e15f0504f3a364a439e98af83dd Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Mon, 13 Sep 2021 13:11:47 -0700 Subject: [PATCH] fixes #1 --- build/debug/gainCalc.m | 36 +++++++------- build/release/gainCalc.m | 36 +++++++------- include/sVars.h | 50 +++++++++---------- include/sim.h | 104 +++++++++++++++++++-------------------- 4 files changed, 113 insertions(+), 113 deletions(-) diff --git a/build/debug/gainCalc.m b/build/debug/gainCalc.m index 0276736..4301efe 100644 --- a/build/debug/gainCalc.m +++ b/build/debug/gainCalc.m @@ -37,24 +37,24 @@ K = Rinv * B' * S; %% Outputs % Copy results in command window to LQRcalc function in C++ -fprintf("float K11 = %3.5f;\n", K(1, 1)) -fprintf("float K12 = %3.5f;\n", K(1, 2)) -fprintf("float K13 = %3.5f;\n", K(1, 3)) -fprintf("float K14 = %3.5f;\n", K(1, 4)) -fprintf("float K15 = %3.5f;\n", K(1, 5)) -fprintf("float K16 = %3.5f;\n", K(1, 6)) -fprintf("float K21 = %3.5f;\n", K(2, 1)) -fprintf("float K22 = %3.5f;\n", K(2, 2)) -fprintf("float K23 = %3.5f;\n", K(2, 3)) -fprintf("float K24 = %3.5f;\n", K(2, 4)) -fprintf("float K25 = %3.5f;\n", K(2, 5)) -fprintf("float K26 = %3.5f;\n", K(2, 6)) -fprintf("float K31 = %3.5f;\n", K(3, 1)) -fprintf("float K32 = %3.5f;\n", K(3, 2)) -fprintf("float K33 = %3.5f;\n", K(3, 3)) -fprintf("float K34 = %3.5f;\n", K(3, 4)) -fprintf("float K35 = %3.5f;\n", K(3, 5)) -fprintf("float K36 = %3.5f;\n", K(3, 6)) +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]'; diff --git a/build/release/gainCalc.m b/build/release/gainCalc.m index 0276736..4301efe 100644 --- a/build/release/gainCalc.m +++ b/build/release/gainCalc.m @@ -37,24 +37,24 @@ K = Rinv * B' * S; %% Outputs % Copy results in command window to LQRcalc function in C++ -fprintf("float K11 = %3.5f;\n", K(1, 1)) -fprintf("float K12 = %3.5f;\n", K(1, 2)) -fprintf("float K13 = %3.5f;\n", K(1, 3)) -fprintf("float K14 = %3.5f;\n", K(1, 4)) -fprintf("float K15 = %3.5f;\n", K(1, 5)) -fprintf("float K16 = %3.5f;\n", K(1, 6)) -fprintf("float K21 = %3.5f;\n", K(2, 1)) -fprintf("float K22 = %3.5f;\n", K(2, 2)) -fprintf("float K23 = %3.5f;\n", K(2, 3)) -fprintf("float K24 = %3.5f;\n", K(2, 4)) -fprintf("float K25 = %3.5f;\n", K(2, 5)) -fprintf("float K26 = %3.5f;\n", K(2, 6)) -fprintf("float K31 = %3.5f;\n", K(3, 1)) -fprintf("float K32 = %3.5f;\n", K(3, 2)) -fprintf("float K33 = %3.5f;\n", K(3, 3)) -fprintf("float K34 = %3.5f;\n", K(3, 4)) -fprintf("float K35 = %3.5f;\n", K(3, 5)) -fprintf("float K36 = %3.5f;\n", K(3, 6)) +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]'; diff --git a/include/sVars.h b/include/sVars.h index 140a3e1..e1649d1 100644 --- a/include/sVars.h +++ b/include/sVars.h @@ -4,38 +4,38 @@ #define SVARS_H struct sVars { - float x, y, z; - float xPrev, yPrev, zPrev; - float vx, vy, vz; - float vxPrev, vyPrev, vzPrev; - float vxBody, vyBody, vzBody; - float ax, ay, az, axPrev, ayPrev, azPrev; + double x, y, z; + double xPrev, yPrev, zPrev; + double vx, vy, vz; + double vxPrev, vyPrev, vzPrev; + double vxBody, vyBody, vzBody; + double ax, ay, az, axPrev, ayPrev, azPrev; - float yaw, pitch, roll; - float phidot, thetadot, psidot; - float yawPrev, pitchPrev, rollPrev; - float yawdot, pitchdot, rolldot; - float yawdotPrev, pitchdotPrev, rolldotPrev; - float yawddot, pitchddot, rollddot; - float yawddotPrev, pitchddotPrev, rollddotPrev; + double yaw, pitch, roll; + double phidot, thetadot, psidot; + double yawPrev, pitchPrev, rollPrev; + double yawdot, pitchdot, rolldot; + double yawdotPrev, pitchdotPrev, rolldotPrev; + double yawddot, pitchddot, rollddot; + double yawddotPrev, pitchddotPrev, rollddotPrev; - float m, m0, mp, mb, mdot; - float vehicleHeight, vehicleRadius, momentArm; + double m, m0, mp, mb, mdot; + double vehicleHeight, vehicleRadius, momentArm; - float tb; - float vb; - float thrust, thrust_prev, burnElapsed, burnStart; - float LQRx, LQRy, Fx, Fy, Fz; - float momentX, momentY, momentZ; + double tb; + double vb; + double thrust, thrust_prev, burnElapsed, burnStart; + double LQRx, LQRy, Fx, Fy, Fz; + double momentX, momentY, momentZ; - float I11, I22, I33; - float I11prev, I22prev, I33prev; - float I11dot, I22dot, I33dot; + double I11, I22, I33; + double I11prev, I22prev, I33prev; + double I11dot, I22dot, I33dot; int maxServo; - float xServoDegs, yServoDegs; + double xServoDegs, yServoDegs; - float simTime; + double simTime; int stepSize; }; diff --git a/include/sim.h b/include/sim.h index 974fd9e..d6774af 100644 --- a/include/sim.h +++ b/include/sim.h @@ -6,13 +6,13 @@ void lqrCalc(struct sVars &); void TVC(struct sVars &); void vehicleDynamics(struct sVars &, int t); void write2CSV(struct sVars &, std::fstream &outfile, int t); -float derivative(float x2, float x1, float dt); -float integral(float x2, float x1, float dt); +double derivative(double x2, double x1, double dt); +double integral(double x2, double x1, double dt); // Any parameters that are constants should be declared here instead of buried // in code -float const dt = 0.001; -float const g = -9.81; +double const dt = 0.001; +double const g = -9.81; void sim(struct sVars &Vars) { @@ -55,12 +55,12 @@ void sim(struct sVars &Vars) { } void burnStartTimeCalc(struct sVars &Vars) { - float v = Vars.vz; - float h = 0; + double v = Vars.vz; + double h = 0; - float a, j, m, thrust; + double a, j, m, thrust; - for (float i = 0.148; i < 3.450; i = i + dt) { + for (double i = 0.148; i < 3.450; i = i + dt) { m = Vars.m0 - i * Vars.mdot; if ((i > 0.147) && (i < 0.420)) @@ -80,12 +80,12 @@ void burnStartTimeCalc(struct sVars &Vars) { Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height Vars.vb = v; // terminal velocity - float burnStartTime = Vars.vb / -g; + double burnStartTime = Vars.vb / -g; Vars.simTime = (Vars.tb + burnStartTime) * 1000; } void thrustSelection(struct sVars &Vars, int t) { - float tol = 0.001; // 0.001 seems to be a nice tolerance + 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) & @@ -130,23 +130,23 @@ void lqrCalc(struct sVars &Vars) { /* - float n = sqrt(398600 / pow(6678, 3)); - float k1 = (Vars.I22 - Vars.I33) / Vars.I11; - float k2 = (Vars.I11 - Vars.I33) / Vars.I22; - float k3 = (Vars.I22 - Vars.I11) / Vars.I33; + 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; - float R11 = pow(10, 2); - float R22 = pow(10, 2); - float R33 = pow(10, 2); + double R11 = pow(10, 2); + double R22 = pow(10, 2); + double R33 = pow(10, 2); - float F11 = -2 * pow(n, 2) * 4 * k1; - float F22 = -2 * pow(n, 2) * 3 * k2; - float F33 = -2 * pow(n, 2) * k3; + double F11 = -2 * pow(n, 2) * 4 * k1; + double F22 = -2 * pow(n, 2) * 3 * k2; + double F33 = -2 * pow(n, 2) * k3; - float G31 = n * (1 - k1); - float G13 = n * (k3 - 1); + double G31 = n * (1 - k1); + double G13 = n * (k3 - 1); - float d = 0.5; + 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. @@ -154,27 +154,27 @@ void lqrCalc(struct sVars &Vars) { https://en.wikipedia.org/wiki/Algebraic_Riccati_equation#Solution */ /* - float gain = 0.25; + double gain = 0.25; - float K11 = + 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)); - float K22 = -1 * gain * Vars.I33 / (R22 * d * Vars.I22); - float K33 = + 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)); - float K34 = + 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)); - float K16 = + 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)); @@ -186,26 +186,26 @@ void lqrCalc(struct sVars &Vars) { */ // Paste in Values from gainCalc.m - float K11 = 39.54316; - float K12 = 0.00000; - float K13 = -0.00000; - float K14 = 39.55769; - float K15 = 0.00000; - float K16 = 0.00000; - float K21 = 0.00000; - float K22 = 39.54316; - float K23 = 0.00000; - float K24 = 0.00000; - float K25 = 39.55769; - float K26 = 0.00000; - float K31 = 0.00000; - float K32 = 0.00000; - float K33 = 39.54316; - float K34 = 0.00000; - float K35 = 0.00000; - float K36 = 39.54394; + 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; - float gain = + double gain = 0.25 * pow(10, -4); // changing exponenet drastically changes results of LQR @@ -430,12 +430,12 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) { outfile << Vars.LQRy << std::endl; } -float derivative(float x2, float x1, float dt) { - float dxdt = (x2 - x1) / (dt / 1000); +double derivative(double x2, double x1, double dt) { + double dxdt = (x2 - x1) / (dt / 1000); return dxdt; } -float integral(float x, float y, float dt) { - float integ = (x * dt / 1000) + y; +double integral(double x, double y, double dt) { + double integ = (x * dt / 1000) + y; return integ; } \ No newline at end of file