Resolve "Separate native and teensy code"

include/sim.h

@@ -4,12 +4,10 @@
 #include <iostream>
 
 void burnStartTimeCalc(struct Vehicle &);
-void thrustSelection(struct Vehicle &, int t);
 void pidController(struct Vehicle &, struct Vehicle &);
 void TVC(struct Vehicle &, struct Vehicle &);
-void vehicleDynamics(struct Vehicle &, struct Vehicle &, int t);
-void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &, int t);
-void write2CSV(struct outVector &, struct Vehicle &, int t);
+void vehicleDynamics(struct Vehicle &, struct Vehicle &);
+void state2vec(struct Vehicle &, struct Vehicle &, struct outVector &);
 double derivative(double current, double previous, double step);
 double integral(double currentChange, double prevValue, double dt);
 double limit(double value, double upr, double lwr);
@@ -19,59 +17,6 @@ double limit(double value, double upr, double lwr);
 double const dt = 0.01;
 double const g = -9.81;
 
-bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
-
-  outVector stateVector;
-
-  // Determine when to burn
-  burnStartTimeCalc(State);
-
-  int t = 0;
-
-  // Start Sim
-  do {
-    vehicleDynamics(State, PrevState, t);
-    thrustSelection(State, t);
-    pidController(State, PrevState);
-    TVC(State, PrevState);
-    state2vec(State, PrevState, stateVector, t);
-    //std::cout << State.vz << "\n";
-
-    t += State.stepSize;
-  } while ((State.z > 0.0));
-std::cout << t << "\n";
-  write2CSV(stateVector, State, t);
-  std::cout << t << "\n";
-
-  bool pass = 1;
-
-  double landing_angle =
-      pow(State.yaw * State.yaw + State.pitch * State.pitch, .5);
-
-  double landing_velocity =
-      pow(State.vx * State.vx + State.vy * State.vy + State.vz * State.vz, .5);
-
-  if (landing_angle < 5.0) {
-    std::cout << "   Landing Angle < 5°    | PASS | ";
-  } else {
-    std::cout << "   Landing Angle < 5°    | FAIL | ";
-    pass = pass * 0;
-  }
-  std::cout << "Final Angles: [" << State.yaw << ", " << State.pitch << "]"
-            << std::endl;
-
-  if (landing_velocity < 5.0) {
-    std::cout << "Landing Velocity < 5 m/s | PASS | ";
-  } else {
-    std::cout << "Landing Velocity < 5 m/s | FAIL | ";
-    pass = pass * 0;
-  }
-  std::cout << "Final Velocity: [" << State.vx << ", " << State.vy << ", "
-            << State.vz << "]" << std::endl;
-
-  return pass;
-}
-
 void burnStartTimeCalc(Vehicle &State) {
   double velocity = State.vz;
   double h = 0;
@@ -96,13 +41,15 @@ void burnStartTimeCalc(Vehicle &State) {
     h = (((thrust / mass) + g) * dt) + h;
   }
   State.z = h + (pow(velocity, 2) / (2 * -g)); // starting height
-  State.burnVelocity = velocity;               // terminal velocity
+  State.z = 18.9;
+  State.burnVelocity = velocity; // terminal velocity
 
   double burnStartTime = State.burnVelocity / -g;
   State.simTime = (State.burntime + burnStartTime) * 1000;
 }
 
-void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
+void vehicleDynamics(Vehicle &State, Vehicle &PrevState) {
+
   // Moment of Inertia
   State.I11 = State.mass * ((1 / 12) * pow(State.vehicleHeight, 2) +
                             pow(State.vehicleRadius, 2) / 4);
@@ -111,7 +58,7 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, int t) {
   State.I33 = State.mass * 0.5 * pow(State.vehicleRadius, 2);
 
   // Idot
-  if (t < 0.1) {
+  if (State.time < 0.1) {
     State.I11dot = 0;
     State.I22dot = 0;
     State.I33dot = 0;
@@ -180,45 +127,6 @@ void vehicleDynamics(Vehicle &State, Vehicle &PrevState, 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
-    // as well as current mass
-    State.burnElapsed = (t - State.burnStart) / 1000;
-    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
-    // Start burn
-    State.burnStart = t;
-    State.burnElapsed = 0;
-  }
-
-  else
-    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
-
-  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
-    State.thrustFiring = true;
-    State.thrust = 65.165 * State.burnElapsed - 2.3921;
-
-  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
-    State.thrust = 0.8932 * pow(State.burnElapsed, 6) -
-                   11.609 * pow(State.burnElapsed, 5) +
-                   60.739 * pow(State.burnElapsed, 4) -
-                   162.99 * pow(State.burnElapsed, 3) +
-                   235.6 * pow(State.burnElapsed, 2) -
-                   174.43 * State.burnElapsed + 67.17;
-
-  else if ((State.burnElapsed > 3.382) && (State.burnElapsed < 3.46))
-    State.thrust = -195.78 * State.burnElapsed - 675.11;
-
-  if (State.burnElapsed > 3.45) {
-    State.thrustFiring = false;
-    State.thrust = 0;
-  }
-}
-
 void pidController(Vehicle &State, struct Vehicle &PrevState) {
   // Make sure we start reacting when we start burning
   if (State.thrust > 0.01) {
@@ -290,23 +198,12 @@ void TVC(Vehicle &State, Vehicle &PrevState) {
         integral(State.xServoDegsDot, PrevState.xServoDegs, State.stepSize);
     State.yServoDegs =
         integral(State.yServoDegsDot, PrevState.yServoDegs, State.stepSize);
-
-    // Vector math to aqcuire thrust vector components
-    State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180));
-    State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180));
-    State.Fz =
-        sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
-        (State.mass * g);
-
-    // Calculate moment created by Fx and Fy
-    State.momentX = State.Fx * State.momentArm;
-    State.momentY = State.Fy * State.momentArm;
-    State.momentZ = 0;
   }
 }
 
-void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
-               int t) {
+void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector) {
+  int t = State.time;
+
   stateVector.x[t] = State.x;
   stateVector.y[t] = State.y;
   stateVector.z[t] = State.z;
@@ -341,63 +238,6 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
   PrevState = State;
 }
 
-void write2CSV(outVector &stateVector, Vehicle &State, int t) {
-
-  // 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,thrustFiring,PIDx,PIDy,thrust"
-          << std::endl;
-
-  // writing to output file
-  for (int i = 0; i < t; i += State.stepSize) {
-    outfile << i << ", ";
-
-    outfile << stateVector.x[i] << ",";
-    outfile << stateVector.y[i] << ",";
-    outfile << stateVector.z[i] << ",";
-
-    outfile << stateVector.vx[i] << ",";
-    outfile << stateVector.vy[i] << ",";
-    outfile << stateVector.vz[i] << ",";
-
-    outfile << stateVector.ax[i] << ",";
-    outfile << stateVector.ay[i] << ",";
-    outfile << stateVector.az[i] << ",";
-
-    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
-    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
-    outfile << stateVector.roll[i] * 180 / M_PI << ",";
-
-    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
-    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
-    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
-
-    outfile << stateVector.servo1[i] << ",";
-    outfile << stateVector.servo2[i] << ",";
-
-    outfile << stateVector.thrustFiring[i] << ",";
-
-    outfile << stateVector.PIDx[i] << ",";
-    outfile << stateVector.PIDy[i] << ",";
-
-    outfile << stateVector.thrust[i] << std::endl;
-  }
-
-  outfile.close();
-  std::cout << "simOut.csv created successfully.\n" << std::endl;
-}
-
 double derivative(double current, double previous, double step) {
   double dxdt = (current - previous) / (step / 1000);
   return dxdt;