code compiles and runs on teensy

include/sim.h

@@ -1,6 +1,10 @@
 #include "Vehicle.h"
 #include "outVector.h"
 
+// #ifdef DARDUINO_TEENSY41
+#include "Arduino.h"
+// #endif
+
 #include <iostream>
 
 void burnStartTimeCalc(struct Vehicle &);
@@ -9,7 +13,7 @@ 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 write2CSV(struct outVector &, struct Vehicle &, int t);
 double derivative(double current, double previous, double step);
 double integral(double currentChange, double prevValue, double dt);
 double limit(double value, double upr, double lwr);
@@ -35,12 +39,12 @@ bool sim(struct Vehicle &State, struct Vehicle &PrevState) {
     pidController(State, PrevState);
     TVC(State, PrevState);
     state2vec(State, PrevState, stateVector, t);
-    //std::cout << State.vz << "\n";
+    // //std::cout << State.vz << "\n";
 
     t += State.stepSize;
   } while ((State.z > 0.0));
-std::cout << t << "\n";
+  std::cout << t << "\n";
-  write2CSV(stateVector, State, t);
+  // write2CSV(stateVector, State, t);
   std::cout << t << "\n";
 
   bool pass = 1;
@@ -91,6 +95,8 @@ void burnStartTimeCalc(Vehicle &State) {
 
     else if ((i > 3.382) && (i < 3.46))
       thrust = -195.78 * i + 675.11;
+    else
+      thrust = 0;
 
     velocity = (((thrust / mass) + g) * dt) + velocity;
     h = (((thrust / mass) + g) * dt) + h;
@@ -341,62 +347,63 @@ void state2vec(Vehicle &State, Vehicle &PrevState, outVector &stateVector,
   PrevState = State;
 }
 
-void write2CSV(outVector &stateVector, Vehicle &State, int t) {
+// void write2CSV(outVector &stateVector, Vehicle &State, int t) {
 
-  // Deleting any previous output file
+//   // Deleting any previous output file
-  if (remove("simOut.csv") != 0)
+//   if (remove("simOut.csv") != 0)
-    perror("No file deletion necessary");
+//     perror("No file deletion necessary");
-  else
+//   else
-    puts("Previous output file successfully deleted");
+//     puts("Previous output file successfully deleted");
 
-  // Define and open output file "simOut.csv"
+//   // Define and open output file "simOut.csv"
-  std::fstream outfile;
+//   std::fstream outfile;
-  outfile.open("simOut.csv", std::ios::app);
+//   outfile.open("simOut.csv", std::ios::app);
 
-  // Output file header. These are the variables that we output - useful for
+//   // Output file header. These are the variables that we output - useful for
-  // debugging
+//   // debugging
-  outfile << "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
+//   outfile <<
-             "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"
+//   "t,x,y,z,vx,vy,vz,ax,ay,az,yaw,pitch,roll,yawdot,pitchdot,rolldot,"
-          << std::endl;
+//              "Servo1,Servo2,thrustFiring,PIDx,PIDy,thrust"
+//           << std::endl;
 
-  // writing to output file
+//   // writing to output file
-  for (int i = 0; i < t; i += State.stepSize) {
+//   for (int i = 0; i < t; i += State.stepSize) {
-    outfile << i << ", ";
+//     outfile << i << ", ";
 
-    outfile << stateVector.x[i] << ",";
+//     outfile << stateVector.x[i] << ",";
-    outfile << stateVector.y[i] << ",";
+//     outfile << stateVector.y[i] << ",";
-    outfile << stateVector.z[i] << ",";
+//     outfile << stateVector.z[i] << ",";
 
-    outfile << stateVector.vx[i] << ",";
+//     outfile << stateVector.vx[i] << ",";
-    outfile << stateVector.vy[i] << ",";
+//     outfile << stateVector.vy[i] << ",";
-    outfile << stateVector.vz[i] << ",";
+//     outfile << stateVector.vz[i] << ",";
 
-    outfile << stateVector.ax[i] << ",";
+//     outfile << stateVector.ax[i] << ",";
-    outfile << stateVector.ay[i] << ",";
+//     outfile << stateVector.ay[i] << ",";
-    outfile << stateVector.az[i] << ",";
+//     outfile << stateVector.az[i] << ",";
 
-    outfile << stateVector.yaw[i] * 180 / M_PI << ",";
+//     outfile << stateVector.yaw[i] * 180 / M_PI << ",";
-    outfile << stateVector.pitch[i] * 180 / M_PI << ",";
+//     outfile << stateVector.pitch[i] * 180 / M_PI << ",";
-    outfile << stateVector.roll[i] * 180 / M_PI << ",";
+//     outfile << stateVector.roll[i] * 180 / M_PI << ",";
 
-    outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
+//     outfile << stateVector.yawdot[i] * 180 / M_PI << ",";
-    outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
+//     outfile << stateVector.pitchdot[i] * 180 / M_PI << ",";
-    outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
+//     outfile << stateVector.rolldot[i] * 180 / M_PI << ",";
 
-    outfile << stateVector.servo1[i] << ",";
+//     outfile << stateVector.servo1[i] << ",";
-    outfile << stateVector.servo2[i] << ",";
+//     outfile << stateVector.servo2[i] << ",";
 
-    outfile << stateVector.thrustFiring[i] << ",";
+//     outfile << stateVector.thrustFiring[i] << ",";
 
-    outfile << stateVector.PIDx[i] << ",";
+//     outfile << stateVector.PIDx[i] << ",";
-    outfile << stateVector.PIDy[i] << ",";
+//     outfile << stateVector.PIDy[i] << ",";
 
-    outfile << stateVector.thrust[i] << std::endl;
+//     outfile << stateVector.thrust[i] << std::endl;
-  }
+//   }
 
-  outfile.close();
+//   outfile.close();
-  std::cout << "simOut.csv created successfully.\n" << std::endl;
+//   //std::cout << "simOut.csv created successfully.\n" << std::endl;
-}
+// }
 
 double derivative(double current, double previous, double step) {
   double dxdt = (current - previous) / (step / 1000);

platformio.ini

@@ -8,7 +8,12 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
+[env:WINDOWS]
+platform = windows_x86
+build_flags = -std=c++11 -Wall -O1
+
 [env:teensy41]
 platform = teensy
 board = teensy41
 framework = arduino
+build_flags = -std=c++11 -Wall -O1 -TEENSY

src/main.cpp

@@ -1,5 +1,10 @@
 #define M_PI 3.14159265359
 
+#ifdef TEENSY
+#endif
+#include <Arduino.h>
+unsigned long tic, toc = millis();
+
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -13,7 +18,7 @@
 
 bool sim(struct Vehicle &);
 
-int main() {
+int run_simulation() {
   Vehicle State;
   Vehicle PrevState;
 
@@ -28,9 +33,9 @@ int main() {
   State.vz = 0; // [m/s]
 
   // Initial YPR
-  State.yaw = 10 * M_PI / 180;   // [rad]
+  State.yaw = 10 * M_PI / 180;  // [rad]
   State.pitch = 5 * M_PI / 180; // [rad]
-  State.roll = 0 * M_PI / 180;   // [rad]
+  State.roll = 0 * M_PI / 180;  // [rad]
 
   // Initial YPRdot
   State.yawdot = 1 * M_PI / 180;    // [rad/s]
@@ -38,7 +43,7 @@ int main() {
   State.rolldot = 0 * M_PI / 180;   // [rad/s]
 
   // Servo Limitation
-  State.maxServo = 7;   // [degs]
+  State.maxServo = 7;       // [degs]
   State.maxServoRate = 360; // [degs/sec]
 
   // Vehicle Properties
@@ -64,5 +69,41 @@ int main() {
   std::cout << std::endl << "Simulation Complete 🚀" << std::endl;
   //                                            ^^^
   // 50% chance this makes Mattys linux crash
-  return 0;
+  return outcome;
+}
+
+#ifndef LED_ON
+#define LED_ON (HIGH)
+#endif
+
+#ifndef LED_BUILTIN
+#define LED_BUILTIN (13)
+#endif
+
+#ifndef LED
+#define LED (LED_BUILTIN)
+#endif
+
+void setup() {
+  // initialize the digital pin as an output.
+  delay(10000);
+  pinMode(LED, OUTPUT);
+  Serial.begin(115200);
+  Serial.println("hello world!");
+}
+
+void loop() {
+
+  Serial.println("blink on");
+  digitalWrite(LED, LED_ON); // set the LED on
+
+  tic = millis();
+  run_simulation();
+  toc = millis();
+
+  Serial.println("SIM RESULT:");
+  Serial.println(toc - tic);
+  Serial.println("blink off");
+  digitalWrite(LED, !LED_ON); // set the LED off
+  delay(1000);
 }