From 952ffaac4ee5ff22c7d0c57d34fdb0b10aac6323 Mon Sep 17 00:00:00 2001 From: Anson Biggs Date: Mon, 13 Sep 2021 14:22:57 -0700 Subject: [PATCH] lots of code cleanup and variable renaming --- .vscode/extensions.json | 3 +- include/sVars.h | 6 ++-- include/sim.h | 78 ++++++++++++++++++++++------------------- src/main.cpp | 11 +++--- 4 files changed, 52 insertions(+), 46 deletions(-) diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 5a9a397..32f52b8 100644 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,5 +1,6 @@ { "recommendations": [ - "ms-vscode.cpptools" + "ms-vscode.cpptools", + "wayou.vscode-todo-highlight" ] } \ No newline at end of file diff --git a/include/sVars.h b/include/sVars.h index e1649d1..7decc21 100644 --- a/include/sVars.h +++ b/include/sVars.h @@ -19,11 +19,11 @@ struct sVars { double yawddot, pitchddot, rollddot; double yawddotPrev, pitchddotPrev, rollddotPrev; - double m, m0, mp, mb, mdot; + double mass, massInitial, massPropellant, massBurnout, mdot; double vehicleHeight, vehicleRadius, momentArm; - double tb; - double vb; + double burntime; + double burnVelocity; double thrust, thrust_prev, burnElapsed, burnStart; double LQRx, LQRy, Fx, Fy, Fz; double momentX, momentY, momentZ; diff --git a/include/sim.h b/include/sim.h index 7d3692d..61b89d9 100644 --- a/include/sim.h +++ b/include/sim.h @@ -18,7 +18,7 @@ void sim(struct sVars &Vars) { // defining a few random values here cause I'm lazy Vars.burnElapsed = 2000; - Vars.m = Vars.m0; + Vars.mass = Vars.massInitial; Vars.thrust_prev = 0; burnStartTimeCalc(Vars); @@ -48,20 +48,22 @@ void sim(struct sVars &Vars) { lqrCalc(Vars); TVC(Vars); vehicleDynamics(Vars, t); - write2CSV(Vars, outfile, t); + write2CSV(Vars, outfile, + t); // TODO: Need to append to memory instead of writing each step } outfile.close(); } void burnStartTimeCalc(struct sVars &Vars) { - double v = Vars.vz; + double velocity = Vars.vz; double h = 0; - double a, j, m, thrust; + double a, j, mass, thrust; + // Piecewise functions for F15 thrust curve for (double i = 0.148; i < 3.450; i = i + dt) { - m = Vars.m0 - i * Vars.mdot; + mass = Vars.massInitial - i * Vars.mdot; if ((i > 0.147) && (i < 0.420)) thrust = 65.165 * i - 2.3921; @@ -73,29 +75,30 @@ void burnStartTimeCalc(struct sVars &Vars) { else if ((i > 3.382) && (i < 3.46)) thrust = -195.78 * i + 675.11; - v = (((thrust / m) + g) * dt) + v; - h = v * dt + h; + velocity = (((thrust / mass) + g) * dt) + velocity; + h = velocity * dt + h; } - Vars.z = h + (pow(v, 2) / (2 * -g)); // starting height - Vars.vb = v; // terminal velocity + Vars.z = h + (pow(velocity, 2) / (2 * -g)); // starting height + Vars.burnVelocity = velocity; // terminal velocity - double burnStartTime = Vars.vb / -g; - Vars.simTime = (Vars.tb + burnStartTime) * 1000; + double burnStartTime = Vars.burnVelocity / -g; + Vars.simTime = (Vars.burntime + burnStartTime) * 1000; } void thrustSelection(struct sVars &Vars, int t) { + // TODO: Determine a real 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) & - (Vars.vb > (1 - tol) * Vars.vz * -1); + bool b_burnStart = (Vars.burnVelocity < (1 + tol) * Vars.vz * -1) & + (Vars.burnVelocity > (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); + Vars.mass = Vars.massInitial - (Vars.mdot * Vars.burnElapsed); } else if (b_burnStart) { @@ -104,7 +107,7 @@ void thrustSelection(struct sVars &Vars, int t) { Vars.burnElapsed = 0; } - else + else // TODO: Make this not arbitrary Vars.burnElapsed = 2000; // arbitrary number to ensure we don't burn if ((Vars.burnElapsed > 0.147) && (Vars.burnElapsed < 0.420)) @@ -122,11 +125,11 @@ void thrustSelection(struct sVars &Vars, int t) { 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); + Vars.I11 = Vars.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) + + pow(Vars.vehicleRadius, 2) / 4); + Vars.I22 = Vars.mass * ((1 / 12) * pow(Vars.vehicleHeight, 2) + + pow(Vars.vehicleRadius, 2) / 4); + Vars.I33 = Vars.mass * 0.5 * pow(Vars.vehicleRadius, 2); // Paste in Values from gainCalc.m double K11 = 39.54316; @@ -148,9 +151,8 @@ void lqrCalc(struct sVars &Vars) { double K35 = 0.00000; double K36 = 39.54394; - double gain = - 0.25 * - pow(10, -4); // changing exponenet drastically changes results of LQR + // changing gain exponent drastically changes results of LQR + double gain = 0.25 * pow(10, -4); // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm Vars.LQRx = @@ -182,14 +184,12 @@ void TVC(struct sVars &Vars) { // Define forces and moments for t = 0 Vars.Fx = 0; Vars.Fy = 0; - Vars.Fz = g * Vars.m0; + Vars.Fz = g * Vars.massInitial; Vars.momentX = 0; Vars.momentY = 0; Vars.momentZ = 0; - } - - else { + } else { // Convert servo position to degrees for comparison to max allowable Vars.xServoDegs = (180 / M_PI) * asin(Vars.LQRx / Vars.thrust); @@ -212,7 +212,7 @@ void TVC(struct sVars &Vars) { Vars.Fx = Vars.thrust * sin(Vars.xServoDegs * (M_PI / 180)); Vars.Fy = Vars.thrust * sin(Vars.yServoDegs * (M_PI / 180)); Vars.Fz = sqrt(pow(Vars.thrust, 2) - (pow(Vars.Fx, 2) + pow(Vars.Fy, 2))) + - (Vars.m * g); + (Vars.mass * g); // Calculate moment created by Fx and Fy Vars.momentX = Vars.Fx * Vars.momentArm; @@ -223,7 +223,7 @@ void TVC(struct sVars &Vars) { void vehicleDynamics(struct sVars &Vars, int t) { // Idot - if (t < 1) { + if (t < 1) { // TODO: Initial conditions should be set at sim start Vars.I11dot = 0; Vars.I22dot = 0; Vars.I33dot = 0; @@ -253,7 +253,7 @@ void vehicleDynamics(struct sVars &Vars, int t) { Vars.ax = 0; Vars.ay = 0; - Vars.az = Vars.Fz / Vars.m0; + Vars.az = Vars.Fz / Vars.massInitial; } else { @@ -263,12 +263,12 @@ void vehicleDynamics(struct sVars &Vars, int t) { 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); + Vars.ax = (Vars.Fx / Vars.mass) + + (Vars.pitchdot * Vars.vz - Vars.rolldot * Vars.vy); + Vars.ay = (Vars.Fy / Vars.mass) + + (Vars.rolldot * Vars.vx - Vars.vz * Vars.yawdot); + Vars.az = (Vars.Fz / Vars.mass) + + (Vars.vy * Vars.yawdot - Vars.pitchdot * Vars.vx); // vx vy vz in Body frame Vars.vx = integral(Vars.ax, Vars.vxPrev, Vars.stepSize); @@ -295,6 +295,9 @@ void vehicleDynamics(struct sVars &Vars, int t) { Vars.roll = integral(Vars.psidot, Vars.rollPrev, Vars.stepSize); } + // TODO: Maybe we should just have a `Vars` for the last time step and at the + // end of each time step make a copy of it + // Set "prev" values for next timestep Vars.I11prev = Vars.I11; Vars.I22prev = Vars.I22; @@ -364,7 +367,7 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) { outfile << Vars.xServoDegs << ", "; outfile << Vars.yServoDegs << ", "; - outfile << Vars.m << ", "; + outfile << Vars.mass << ", "; outfile << Vars.thrust << ", "; outfile << Vars.burnElapsed << ", "; outfile << Vars.Fz << ", "; @@ -374,6 +377,7 @@ void write2CSV(struct sVars &Vars, std::fstream &outfile, int t) { } double derivative(double x2, double x1, double dt) { + // TODO: is 1000 arbitrary? double dxdt = (x2 - x1) / (dt / 1000); return dxdt; } diff --git a/src/main.cpp b/src/main.cpp index be4e4de..4551a8c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -30,19 +30,20 @@ int main() { Vars.maxServo = 15; // [degs] // Vehicle Properties - Vars.m0 = 1.2; // [kg] + Vars.massInitial = 1.2; // [kg] Vars.vehicleHeight = 0.5318; // [m] Vars.vehicleRadius = 0.05105; // [m] Vars.momentArm = 0.145; // [m] // Sim Step Size + // TODO: use dt instead? Vars.stepSize = 1; // [ms] // Other Properties - Vars.mp = 0.06; // [kg] - Vars.mb = Vars.m0 - Vars.mp; // [kg] - Vars.tb = 3.45 - 0.148; // [s] - Vars.mdot = Vars.mp / Vars.tb; // [kg/s] + Vars.massPropellant = 0.06; // [kg] + Vars.massBurnout = Vars.massInitial - Vars.massPropellant; // [kg] + Vars.burntime = 3.45 - 0.148; // [s] + Vars.mdot = Vars.massPropellant / Vars.burntime; // [kg/s] sim(Vars);