diff --git a/.vscode/settings.json b/.vscode/settings.json index 72bdb17..a124363 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -38,7 +38,8 @@ "xstddef": "cpp", "xstring": "cpp", "xtr1common": "cpp", - "xutility": "cpp" + "xutility": "cpp", + "vector": "cpp" }, "C_Cpp.clang_format_fallbackStyle": "LLVM", "editor.formatOnSave": true, diff --git a/include/Vehicle.h b/include/Vehicle.h index a13596a..654531f 100644 --- a/include/Vehicle.h +++ b/include/Vehicle.h @@ -1,7 +1,7 @@ #include -#ifndef SVARS_H -#define SVARS_H +#ifndef VEHICLE_H +#define VEHICLE_H struct Vehicle { double x, y, z; @@ -33,4 +33,4 @@ struct Vehicle { int stepSize; }; -#endif +#endif \ No newline at end of file diff --git a/include/outVector.h b/include/outVector.h new file mode 100644 index 0000000..f4550d9 --- /dev/null +++ b/include/outVector.h @@ -0,0 +1,34 @@ +#include + +#ifndef OUTVECTOR_H +#define OUTVECTOR_H + + +struct outVector { + int length = 10000; // current sim runs ~5000 steps, x2 just in case + + std::vector x = std::vector(length, 0.0); + std::vector y = std::vector(length, 0.0); + std::vector z = std::vector(length, 0.0); + + std::vector vx = std::vector(length, 0.0); + std::vector vy = std::vector(length, 0.0); + std::vector vz = std::vector(length, 0.0); + + std::vector ax = std::vector(length, 0.0); + std::vector ay = std::vector(length, 0.0); + std::vector az = std::vector(length, 0.0); + + std::vector yaw = std::vector(length, 0.0); + std::vector pitch = std::vector(length, 0.0); + std::vector roll = std::vector(length, 0.0); + + std::vector yawdot = std::vector(length, 0.0); + std::vector pitchdot = std::vector(length, 0.0); + std::vector rolldot = std::vector(length, 0.0); + + std::vector servo1 = std::vector(length, 0.0); + std::vector servo2 = std::vector(length, 0.0); +}; + +#endif \ No newline at end of file diff --git a/include/sim.h b/include/sim.h index 6d864bd..fe0ce63 100644 --- a/include/sim.h +++ b/include/sim.h @@ -1,11 +1,13 @@ #include "Vehicle.h" +#include "outVector.h" void burnStartTimeCalc(struct Vehicle &); void thrustSelection(struct Vehicle &, int t); void lqrCalc(struct Vehicle &); void TVC(struct Vehicle &); void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t); -void write2CSV(struct Vehicle &, std::fstream &outfile, int t); +void state2vec(struct Vehicle &, struct outVector &, int t); +void write2CSV(struct outVector &); double derivative(double x2, double x1, double dt); double integral(double x2, double x1, double dt); @@ -16,32 +18,18 @@ double const g = -9.81; bool sim(struct Vehicle &State, struct Vehicle &PrevState) { +<<<<<<< HEAD // defining a few random values here cause I'm lazy State.burnElapsed = 2000; State.mass = State.massInitial; PrevState.thrust = 0.0; +======= + outVector stateVector; +>>>>>>> 3-store-data-in-memory + // Determine when to burn burnStartTimeCalc(State); - // 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; - int t = 0; // Start Sim @@ -50,11 +38,16 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { lqrCalc(State); TVC(State); vehicleDynamics(State, PrevState, t); - write2CSV(State, outfile, t); - t++; - } while ((State.z > 0.0) || (State.thrust > 1.0)); + state2vec(State, stateVector, t); - outfile.close(); + t++; +<<<<<<< HEAD + } while ((State.z > 0.0) || (State.thrust > 1.0)); +======= + } while ((State.z > 0) || (State.thrust > 1)); +>>>>>>> 3-store-data-in-memory + + write2CSV(stateVector); bool returnValue; @@ -71,7 +64,7 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) { return returnValue; } -void burnStartTimeCalc(struct Vehicle &State) { +void burnStartTimeCalc(Vehicle &State) { double velocity = State.vz; double h = 0; @@ -102,7 +95,7 @@ void burnStartTimeCalc(struct Vehicle &State) { State.simTime = (State.burntime + burnStartTime) * 1000; } -void thrustSelection(struct Vehicle &State, int t) { +void thrustSelection(Vehicle &State, int t) { if (State.burnElapsed != 2000) { // determine where in the thrust curve we're at based on elapsed burn time @@ -135,7 +128,7 @@ void thrustSelection(struct Vehicle &State, int t) { State.thrust = -195.78 * State.burnElapsed + 675.11; } -void lqrCalc(struct Vehicle &State) { +void lqrCalc(Vehicle &State) { State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) + pow(State.vehicleRadius, 2) / 4); @@ -166,7 +159,8 @@ void lqrCalc(struct Vehicle &State) { // 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 + // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment + // arm State.LQRx = gain * ((K12 * State.pitch) / 2 + K15 * State.pitchdot + (K13 * State.roll) / 2 + @@ -191,7 +185,7 @@ void lqrCalc(struct Vehicle &State) { State.LQRy = -1 * State.thrust; } -void TVC(struct Vehicle &State) { +void TVC(Vehicle &State) { if (State.thrust < 1) { // Define forces and moments for t = 0 State.Fx = 0; @@ -309,86 +303,84 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) { State.roll = integral(State.psidot, PrevState.roll, State.stepSize); } - PrevState = State; - - /* // Set "prev" values for next timestep - PrevState.I11 = State.I11; - PrevState.I22 = State.I22; - PrevState.I33 = State.I33; - - PrevState.yaw = State.yaw; - PrevState.pitch = State.pitch; - PrevState.roll = State.roll; - - PrevState.yawdot = State.yawdot; - PrevState.pitchdot = State.pitchdot; - PrevState.rolldot = State.rolldot; - - PrevState.yawddot = State.yawddot; - PrevState.pitchddot = State.pitchddot; - PrevState.rollddot = State.rollddot; - - PrevState.ax = State.ax; - PrevState.ay = State.ay; - PrevState.az = State.az; - - PrevState.vx = State.vx; - PrevState.vy = State.vy; - PrevState.vz = State.vz; - - PrevState.x = State.x; - PrevState.y = State.y; - PrevState.z = State.z; - */ + PrevState = State; } -void write2CSV(struct Vehicle &State, std::fstream &outfile, int t) { +void state2vec(Vehicle &State, outVector &stateVector, int t) { + stateVector.x[t] = State.x; + stateVector.y[t] = State.y; + stateVector.z[t] = State.z; + + stateVector.vx[t] = State.vx; + stateVector.vy[t] = State.vy; + stateVector.vz[t] = State.vz; + + stateVector.ax[t] = State.ax; + stateVector.ay[t] = State.ay; + stateVector.az[t] = State.az; + + stateVector.yaw[t] = State.yaw; + stateVector.pitch[t] = State.pitch; + stateVector.roll[t] = State.roll; + + stateVector.yawdot[t] = State.yawdot; + stateVector.pitchdot[t] = State.pitchdot; + stateVector.rolldot[t] = State.rolldot; + + stateVector.servo1[t] = State.xServoDegs; + stateVector.servo2[t] = State.yServoDegs; +} + +void write2CSV(outVector &stateVector) { + + // Deleting any previous output file + if (remove("simOut.csv") != 0) + perror("No file deletion necessary"); + else + puts("Previous output 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, Servo1, Servo2" + << std::endl; + + std::cout << "Writing to csv...\n"; + // writing to output file - outfile << t << ", "; + for (int t = 0; t < stateVector.x.size(); t++) { + outfile << t << ", "; - outfile << State.x << ", "; - outfile << State.y << ", "; - outfile << State.z << ", "; + outfile << stateVector.x[t] << ", "; + outfile << stateVector.y[t] << ", "; + outfile << stateVector.z[t] << ", "; - outfile << State.vx << ", "; - outfile << State.vy << ", "; - outfile << State.vz << ", "; + outfile << stateVector.vx[t] << ", "; + outfile << stateVector.vy[t] << ", "; + outfile << stateVector.vz[t] << ", "; - outfile << State.ax << ", "; - outfile << State.ay << ", "; - outfile << State.az << ", "; + outfile << stateVector.ax[t] << ", "; + outfile << stateVector.ay[t] << ", "; + outfile << stateVector.az[t] << ", "; - outfile << State.yaw * 180 / M_PI << ", "; - outfile << State.pitch * 180 / M_PI << ", "; - outfile << State.roll * 180 / M_PI << ", "; + outfile << stateVector.yaw[t] * 180 / M_PI << ", "; + outfile << stateVector.pitch[t] * 180 / M_PI << ", "; + outfile << stateVector.roll[t] * 180 / M_PI << ", "; - outfile << State.yawdot * 180 / M_PI << ", "; - outfile << State.pitchdot * 180 / M_PI << ", "; - outfile << State.rolldot * 180 / M_PI << ", "; + outfile << stateVector.yawdot[t] * 180 / M_PI << ", "; + outfile << stateVector.pitchdot[t] * 180 / M_PI << ", "; + outfile << stateVector.rolldot[t] * 180 / M_PI << ", "; - outfile << State.yawddot * 180 / M_PI << ", "; - outfile << State.pitchddot * 180 / M_PI << ", "; - outfile << State.rollddot * 180 / M_PI << ", "; + outfile << stateVector.servo1[t] << ", "; + outfile << stateVector.servo2[t] << std::endl; + } - outfile << State.I11 << ", "; - outfile << State.I22 << ", "; - outfile << State.I33 << ", "; - - outfile << State.I11dot << ", "; - outfile << State.I22dot << ", "; - outfile << State.I33dot << ", "; - - outfile << State.xServoDegs << ", "; - outfile << State.yServoDegs << ", "; - - outfile << State.mass << ", "; - outfile << State.thrust << ", "; - outfile << State.burnElapsed << ", "; - outfile << State.Fz << ", "; - - outfile << State.LQRx << ", "; - outfile << State.LQRy << std::endl; + outfile.close(); } double derivative(double x2, double x1, double dt) { diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m index 959e38b..047c0cb 100644 --- a/matlabHelpers/simPlot.m +++ b/matlabHelpers/simPlot.m @@ -3,34 +3,29 @@ clear all; close all; clc; % Read data and transfer to variables T = readmatrix('../simOut.csv'); t = T(:, 1); + x = T(:, 2); y = T(:, 3); z = T(:, 4); + vx = T(:, 5); vy = T(:, 6); vz = T(:, 7); + ax = T(:, 8); ay = T(:, 9); az = T(:, 10); + yaw = T(:, 11); pitch = T(:, 12); roll = T(:, 13); + yawdot = T(:, 14); pitchdot = T(:, 15); rolldot = T(:, 16); -yawddot = T(:, 17); -pitchddot = T(:, 18); -rollddot = T(:, 19); -I11 = T(:, 20); -I22 = T(:, 21); -I33 = T(:, 22); -I11dot = T(:, 23); -I22dot = T(:, 24); -I33dot = T(:, 25); -Servo1 = T(:, 26); -Servo2 = T(:, 27); -m = T(:, 28); -thrust = T(:, 29); + +Servo1 = T(:, 17); +Servo2 = T(:, 18); % Acceleration subplot(3, 1, 1) diff --git a/src/main.cpp b/src/main.cpp index c9d614a..d00e337 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -27,7 +27,9 @@ int main() { // Initial YPRdot State.yawdot = 0 * M_PI / 180; // [rad/s] State.pitchdot = 0 * M_PI / 180; // [rad/s] - State.rolldot = 0 * M_PI / 180; // [rad/s] + State.rolldot = + 0 * M_PI / + 180; // [rad/s] std::vector vec = std::vector(10); // Servo Limitation State.maxServo = 15; // [degs] @@ -46,6 +48,9 @@ int main() { State.massBurnout = State.massInitial - State.massPropellant; // [kg] State.burntime = 3.45 - 0.148; // [s] State.mdot = State.massPropellant / State.burntime; // [kg/s] + State.mass = State.massInitial; // [kg] + State.burnElapsed = 2000; // [s] + PrevState.thrust = 0; // [N] bool outcome = sim(State, PrevState); @@ -53,9 +58,9 @@ int main() { << "\n"; if (outcome == 1) - std::cout << "Success!"; + std::cout << "Sim Result = Success!"; else if (outcome == 0) - std::cout << "Failed!"; + std::cout << "Sim Result = Failed!"; return 0; } \ No newline at end of file