Resolved #3

.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,

build/debug/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)

include/Vehicle.h

@@ -1,7 +1,7 @@
 #include <array>
 
-#ifndef SVARS_H
-#define SVARS_H
+#ifndef VEHICLE_H
+#define VEHICLE_H
 
 struct Vehicle {
   double x, y, z;

include/outVector.h

@@ -0,0 +1,34 @@
+#include <vector>
+
+#ifndef OUTVECTOR_H
+#define OUTVECTOR_H
+
+
+struct outVector {
+  int length = 10000; // current sim runs ~5000 steps, x2 just in case
+
+  std::vector<double> x = std::vector<double>(length, 0.0);
+  std::vector<double> y = std::vector<double>(length, 0.0);
+  std::vector<double> z = std::vector<double>(length, 0.0);
+
+  std::vector<double> vx = std::vector<double>(length, 0.0);
+  std::vector<double> vy = std::vector<double>(length, 0.0);
+  std::vector<double> vz = std::vector<double>(length, 0.0);
+
+  std::vector<double> ax = std::vector<double>(length, 0.0);
+  std::vector<double> ay = std::vector<double>(length, 0.0);
+  std::vector<double> az = std::vector<double>(length, 0.0);
+
+  std::vector<double> yaw = std::vector<double>(length, 0.0);
+  std::vector<double> pitch = std::vector<double>(length, 0.0);
+  std::vector<double> roll = std::vector<double>(length, 0.0);
+
+  std::vector<double> yawdot = std::vector<double>(length, 0.0);
+  std::vector<double> pitchdot = std::vector<double>(length, 0.0);
+  std::vector<double> rolldot = std::vector<double>(length, 0.0);
+
+  std::vector<double> servo1 = std::vector<double>(length, 0.0);
+  std::vector<double> servo2 = std::vector<double>(length, 0.0);
+};
+
+#endif

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,11 @@ double const g = -9.81;
 
 bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
 
-  // defining a few random values here cause I'm lazy
-  State.burnElapsed = 2000;
-  State.mass = State.massInitial;
-  PrevState.thrust = 0;
+  outVector stateVector;
 
+  // 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 +31,12 @@ 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) && (State.thrust != 0));
+    state2vec(State, stateVector, t);
 
-  outfile.close();
+    t++;
+  } while ((State.z > 0) || (State.thrust > 1));
+
+  write2CSV(stateVector);
 
   bool returnValue;
 
@@ -71,7 +53,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 +84,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 +117,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 +148,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 +174,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 +292,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
+  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) {

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 <int> vec = std::vector<int>(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;
 }