From ee67077664c5f25c4b0b4b0f31f0dbb5f98a1d52 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sat, 18 Sep 2021 16:26:54 -0700
Subject: [PATCH] Resolved #7 - added inputs.csv for initial conditions

---
 matlabHelpers/simPlot.m |  6 +--
 src/main.cpp            | 88 +++++++++++++++++++++++++----------------
 2 files changed, 57 insertions(+), 37 deletions(-)

diff --git a/matlabHelpers/simPlot.m b/matlabHelpers/simPlot.m
index 047c0cb..f15e1c6 100644
--- a/matlabHelpers/simPlot.m
+++ b/matlabHelpers/simPlot.m
@@ -48,7 +48,7 @@ title('Altitude vs Time')
 xlabel('Time (s)')
 ylabel('Altitude (m)')
 ylim([0 z(1)+5])
-saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
+%saveas(gcf,'outputs/Accel-Vel-Alt vs Time.png')
 
 figure(2)
 
@@ -72,7 +72,7 @@ plot(t, rolldot)
 title('Angular Velocity vs Time')
 xlabel('Time (ms)')
 ylabel('Angular Velocity (deg/s)')
-saveas(gcf,'outputs/Euler Angles vs Time.png')
+%saveas(gcf,'outputs/Euler Angles vs Time.png')
 legend("yawdot", "pitchdot", "rolldot")
 
 figure(3)
@@ -90,4 +90,4 @@ plot(t, Servo2)
 title('Servo 2 Position vs Time')
 xlabel('Time (ms)')
 ylabel('Servo 2 Position (rad)')
-saveas(gcf,'outputs/Servo Position vs Time.png')
+%saveas(gcf,'outputs/Servo Position vs Time.png')
diff --git a/src/main.cpp b/src/main.cpp
index 50c2e3e..c1a7562 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -3,7 +3,10 @@
 #include <cmath>
 #include <fstream>
 #include <iostream>
+#include <string>
+#include <stdexcept> // std::runtime_error
 #include <stdio.h>
+#include <vector>
 
 #include "Vehicle.h"
 #include "sim.h"
@@ -14,50 +17,67 @@ int main() {
   Vehicle State;
   Vehicle PrevState;
 
+  // Create an input filestream
+  std::ifstream inFile("input.csv");
+
+  // Make sure the file is open
+  if (!inFile.is_open())
+    throw std::runtime_error("Could not open file");
+
+  std::vector<double> varValueVec = std::vector<double>(17, 0.0);
+  std::string varName, varValue, varUnits;
+
+  for (int i; i < 17; i++) {
+    std::getline(inFile, varName, ',');
+    std::getline(inFile, varValue, ',');
+    varValueVec[i] = stod(varValue);
+    std::getline(inFile, varUnits);
+    }
+
   // Initial Velocity
-  State.vx = 0; // [m/s]
-  State.vy = 0; // [m/s]
-  State.vz = 0; // [m/s]
+    State.vx = varValueVec[0]; // [m/s]
+    State.vy = varValueVec[1]; // [m/s]
+    State.vz = varValueVec[2]; // [m/s]
 
-  // Initial YPR
-  State.yaw = 10 * M_PI / 180;  // [rad]
-  State.pitch = 5 * M_PI / 180; // [rad]
-  State.roll = 0 * M_PI / 180;  // [rad]
+    // Initial YPR
+    State.yaw = varValueVec[3] * M_PI / 180; // [rad]
+    State.pitch = varValueVec[4] * M_PI / 180; // [rad]
+    State.roll = varValueVec[5] * M_PI / 180;  // [rad]
 
-  // Initial YPRdot
-  State.yawdot = 1 * M_PI / 180;    // [rad/s]
-  State.pitchdot = -1 * M_PI / 180; // [rad/s]
-  State.rolldot = 0 * M_PI / 180;   // [rad/s]
+    // Initial YPRdot
+    State.yawdot = varValueVec[6] * M_PI / 180; // [rad/s]
+    State.pitchdot = varValueVec[7] * M_PI / 180; // [rad/s]
+    State.rolldot = varValueVec[8] * M_PI / 180;   // [rad/s]
 
-  // Servo Limitation
-  State.maxServo = 15; // [degs]
+    // Servo Limitation
+    State.maxServo = varValueVec[9]; // [degs]
 
-  // Vehicle Properties
-  State.massInitial = 1.2;       // [kg]
-  State.vehicleHeight = 0.5318;  // [m]
-  State.vehicleRadius = 0.05105; // [m]
-  State.momentArm = 0.145;       // [m]
+    // Vehicle Properties
+    State.massInitial = varValueVec[10]; // [kg]
+    State.vehicleHeight = varValueVec[13]; // [m]
+    State.vehicleRadius = varValueVec[14]; // [m]
+    State.momentArm = varValueVec[15];     // [m]
 
-  // Sim Step Size
-  State.stepSize = 1; // [ms]
+    // Sim Step Size
+    State.stepSize = varValueVec[16]; // [ms]
 
-  // Other Properties
-  State.massPropellant = 0.06;                                  // [kg]
-  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]
+    // Other Properties
+    State.burntime = varValueVec[12];                              // [s]
+    State.massPropellant = varValueVec[11];                        // [kg]
+    State.massBurnout = State.massInitial - State.massPropellant; // [kg]
+    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);
+    bool outcome = sim(State, PrevState);
 
-  std::cout << "Finished"
-            << "\n";
+    std::cout << "Finished"
+              << "\n";
 
-  if (outcome == 1) {
-    std::cout << "Sim Result = Success!";
-    return 0;
+    if (outcome == 1) {
+      std::cout << "Sim Result = Success!";
+      return 0;
   } else if (outcome == 0) {
     std::cout << "Sim Result = Failed!";
     // return 1; Until I figure out how to make CI/CD continue even when run