Fixed differences between native and teensy, added getThrust() function to clean up thrustInfo()

include/Vehicle.h

@@ -20,7 +20,7 @@ struct Vehicle {
   double burntime;
   double burnVelocity;
   double thrust, burnElapsed, burnStart;
-  bool thrustFiring = false;
+  int thrustFiring = 0; // 0: Pre-burn, 1: Mid-burn, 2: Post-burn
 
   double PIDx, PIDy, Fx, Fy, Fz;
   double momentX, momentY, momentZ;

include/native.h

@@ -8,41 +8,23 @@ void write2CSV(struct outVector &, struct Vehicle &);
 void printSimResults(struct Vehicle &);
 
 void thrustInfo(Vehicle &State) {
-
+  if ((std::abs(State.burnVelocity + State.vz) < 1.03) &&
-  if (State.burnElapsed != 2000) {
+      (State.thrustFiring == 0)) {
-    // determine where in the thrust curve we're at based on elapsed burn time
-    // as well as current mass
-    State.burnElapsed = (State.time - State.burnStart) / 1000;
-    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
-  }
-
-  else if (abs(State.burnVelocity + State.vz) < 0.001) {
     // Start burn
     State.burnStart = State.time;
-    State.burnElapsed = 0;
+    State.burnElapsed = 0.0;
-  }
+    State.thrustFiring = 1;
 
-  else
+    getThrust(State);
-    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
 
-  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
+  } else if (State.thrustFiring == 1) {
-    State.thrustFiring = true;
+    State.burnElapsed = (State.time - State.burnStart) / 1000.0;
-    State.thrust = 65.165 * State.burnElapsed - 2.3921;
+    State.mass = State.massInitial - (State.mdot * State.burnElapsed);
 
-  } else if ((State.burnElapsed > 0.419) && (State.burnElapsed < 3.383))
+    getThrust(State);
-    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))
+  } else {
-    State.thrust = -195.78 * State.burnElapsed - 675.11;
+    State.thrust = 0.0;
-
-  if (State.burnElapsed > 3.45) {
-    State.thrustFiring = false;
-    State.thrust = 0;
   }
 }
 

include/sim.h

@@ -11,6 +11,7 @@ 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);
+void getThrust(struct Vehicle &);
 
 // Any parameters that are constants should be declared here instead of
 // buried in code
@@ -254,4 +255,26 @@ double limit(double value, double upr, double lwr) {
     value = value;
 
   return value;
+}
+
+void getThrust(Vehicle &State) {
+  if ((State.burnElapsed > 0.147) && (State.burnElapsed < 0.420)) {
+    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.0) -
+                   11.609 * pow(State.burnElapsed, 5.0) +
+                   60.739 * pow(State.burnElapsed, 4.0) -
+                   162.99 * pow(State.burnElapsed, 3.0) +
+                   235.6 * pow(State.burnElapsed, 2.0) -
+                   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 = 2;
+    State.thrust = 0.0;
+  }
 }

include/teensy.h

@@ -35,7 +35,6 @@ void initFile() {
   Serial.println("Card initialized.");
 
   int i = 1;
-  Serial.print("simOut_" + String(i) + ".csv");
   const char *fileName;
 
   if (SD.exists("simOut.csv")) {
@@ -84,38 +83,22 @@ void thrustInfo(Vehicle &State) {
     Serial.println("WARNING: thrustInfo not implemented for TEENSY");
   }
 
-  if (State.burnElapsed != 2000) {
+  if ((abs(State.burnVelocity + State.vz) < 1.03) &&
-    // determine where in the thrust curve we're at based on elapsed burn time
+      (State.thrustFiring == 0)) {
-    // as well as current mass
+    // Start burn
+    State.burnStart = State.time;
+    State.burnElapsed = 0.0;
+    State.thrustFiring = 1;
+
+    getThrust(State);
+
+  } else if (State.thrustFiring == 1) {
     State.burnElapsed = (State.time - State.burnStart) / 1000.0;
     State.mass = State.massInitial - (State.mdot * State.burnElapsed);
 
-  } else if (abs(State.burnVelocity + State.vz) < 0.01) {
+    getThrust(State);
-    // Start burn
-    State.burnStart = State.time;
-    State.burnElapsed = 0;
 
   } else {
-    State.burnElapsed = 2000; // arbitrary number to ensure we don't burn
-  }
-  // Serial.println(abs(State.burnVelocity + State.vz));
-
-  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.0;
   }
 }

matlabHelpers/simPlot.m

@@ -30,6 +30,7 @@ Servo2      = T(:, 18);
 PIDx      = T(:, 20);
 PIDy      = T(:, 21);
 
+thrust = T(:, 22);
 % Acceleration
 subplot(3, 1, 1)
 plot(t, az)
@@ -91,3 +92,12 @@ plot(t, Servo2)
 title('Servo 2 Position vs Time')
 xlabel('Time (ms)')
 ylabel('Servo 2 Position (rad)')
+
+figure(4)
+
+% Servo 1 Position
+
+plot(t, thrust)
+title('Thrust vs Time')
+xlabel('Time (ms)')
+ylabel('Thrust (N)')

src/main.cpp

@@ -36,14 +36,13 @@ void setup() {
 }
 #elif defined(TEENSY)
 void setup() {
-  delay(1000);
+  delay(5000);
   init_Vehicle(State);
   Serial.println("Simulated Vehicle Initalized");
   delay(1000);
 
   // Determine when to burn
   burnStartTimeCalc(State);
-  Serial.println(State.burnVelocity);
   Serial.println("Starting Height Calculated");
   delay(1000);
   loadCellCalibrate();
@@ -65,10 +64,9 @@ void loop() {
 
   // Set "prev" values for next timestep
   PrevState = State;
-
   State.time += State.stepSize;
 
-  if (State.z < 0.0) {
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     write2CSV(stateVector, State);
     printSimResults(State);
     init_Vehicle(State);
@@ -90,14 +88,14 @@ void loop() {
 
   State.time += State.stepSize;
 
-  if (State.z < 0.0) {
+  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
     printSimResults(State);
     init_Vehicle(State);
     delay(1000);
     Serial.println("Last run duration:" + String(millis() - last) + " ms");
 
     closeFile();
-    delay(10000);
+    delay(20000);
 
     Serial.println("Restarting Sim");
     Serial.println(
@@ -113,7 +111,7 @@ int main() {
 
   do {
     loop();
-  } while ((State.z > 0.0));
+  } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
   return 0;
 }