organized main

src/main.cpp

@@ -1,6 +1,4 @@
-#define M_PI 3.14159265359
+#if defined(_WIN32) || defined(NATIVE)
-
-#if defined(NATIVE) || defined(_WIN32)
 #include <cmath>
 #include <fstream>
 #include <iostream>
@@ -10,28 +8,31 @@
 #include <vector>
 #elif defined(TEENSY)
 #include <Arduino.h>
-
-int BUILTIN_LED = 13;
-unsigned long last, initTime;
 #endif
 
+#define M_PI 3.14159265359
+
 #include "Vehicle.h"
 #include "sim.h"
 
-#if defined(NATIVE) || defined(_WIN32)
+Vehicle State;
-#include "native.h"
+Vehicle PrevState;
-outVector stateVector;
+
-#elif defined(TEENSY)
+#if defined(TEENSY)
+
+int BUILTIN_LED = 13;
+unsigned long last, initTime;
 
 #include "teensy.h"
 #include <HX711.h>
 const int lc_clock = 23;
 
-const int lc_data_0 = 14;
+const int pin_lc0 = 14;
-const int lc_data_1 = 15;
+const int pin_lc1 = 15;
-const int lc_data_2 = 19;
+const int pin_lc2 = 19;
-const int lc_data_3 = 20;
+const int pin_lc3 = 20;
 
+void init_loadCells();
 void read_lc0();
 void read_lc1();
 void read_lc2();
@@ -42,96 +43,30 @@ HX711 lc1;
 HX711 lc2;
 HX711 lc3;
 
-#endif
-
-Vehicle State;
-Vehicle PrevState;
-
-#if defined(NATIVE) || defined(_WIN32)
-void setup() {
-  init_Vehicle(State);
-
-  // Determine when to burn
-  burnStartTimeCalc(State);
-}
-#elif defined(TEENSY)
 void setup() {
   Serial.begin(9600);
 
-  // Configure clock pin with high impedance to protect pin (if this doesn't
+  delay(5000);
-  // work, change to OUTPUT)
+  Serial.println("Simulation Countdown:") for (int i = 0; i < 15; i++) {
-  pinMode(lc_clock, INPUT);
+    Serial.println(15 - i) delay(1000);
-  // Configure load cell data pins as inputs
+  }
-  pinMode(lc_data_0, INPUT);
-  pinMode(lc_data_1, INPUT);
-  pinMode(lc_data_2, INPUT);
-  pinMode(lc_data_3, INPUT);
-  lc0.begin(lc_data_0, lc_clock);
-  lc1.begin(lc_data_1, lc_clock);
-  lc2.begin(lc_data_2, lc_clock);
-  lc3.begin(lc_data_3, lc_clock);
 
-  // Attach ISRs to load cell data pins to read data when available
+  init_loadCells();
-  attachInterrupt(digitalPinToInterrupt(lc_data_0), read_lc0, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_1), read_lc1, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_2), read_lc2, LOW);
-  attachInterrupt(digitalPinToInterrupt(lc_data_3), read_lc3, LOW);
-
-  Serial.println("Load Cells Initialized");
-
-  Serial.print("Calibrating");
-  lc0.tare();
-  Serial.print(".");
-  lc1.tare();
-  Serial.print(".");
-  lc2.tare();
-  Serial.print(".");
-  lc3.tare();
-  Serial.println(".");
-  Serial.println("Load Cells Calibrated");
-
-  delay(1000);
   init_Vehicle(State);
   State.lc0 = lc0.get_value();
   State.lc1 = lc1.get_value();
   State.lc2 = lc2.get_value();
   State.lc3 = lc3.get_value();
-
   Serial.println("Simulated Vehicle Initalized");
-  delay(1000);
 
   // Determine when to burn
   burnStartTimeCalc(State);
   Serial.println("Starting Height Calculated");
-  delay(1000);
 
   initFile();
-  delay(1000);
 
   initTime = micros();
 }
-#endif
-
-#if defined(NATIVE) || defined(_WIN32)
-void loop() {
-  vehicleDynamics(State, PrevState);
-  thrustInfo(State);
-  pidController(State, PrevState);
-  TVC(State, PrevState);
-  processTVC(State);
-  state2vec(State, PrevState, stateVector);
-
-  // Set "prev" values for next timestep
-  PrevState = State;
-  State.time += State.stepSize;
-
-  if ((State.z < 0.0) && (State.thrustFiring == 2)) {
-    write2CSV(stateVector, State);
-    printSimResults(State);
-    init_Vehicle(State);
-  }
-}
-#elif defined(TEENSY)
 void loop() {
 
   last = micros();
@@ -147,10 +82,11 @@ void loop() {
   State.stepDuration = micros() - last;
   write2CSV(State);
 
+  //       \/  Comments are fee you can just write previous bro
   // Set "prev" values for next timestep
   PrevState = State;
 
-  // state2vec(State, PrevState, stateVector);
+  // state2vec(State, PrevState, stateVector); <-- can we delete?
 
   State.time += State.stepSize;
 
@@ -167,26 +103,80 @@ void loop() {
 
   delay(20 - ((micros() - last) * 1000.0));
 }
+
+void init_loadCells() {
+  // Configure clock pin with high impedance to protect
+  // pin (if this doesn't  work, change to OUTPUT)
+  pinMode(lc_clock, INPUT);
+
+  // Configure load cell data pins as inputs
+  pinMode(pin_lc0, INPUT);
+  pinMode(pin_lc1, INPUT);
+  pinMode(pin_lc2, INPUT);
+  pinMode(pin_lc3, INPUT);
+  lc0.begin(pin_lc0, lc_clock);
+  lc1.begin(pin_lc1, lc_clock);
+  lc2.begin(pin_lc2, lc_clock);
+  lc3.begin(pin_lc3, lc_clock);
+
+  // Attach ISRs to load cell data pins to read data when available
+  attachInterrupt(digitalPinToInterrupt(pin_lc0), read_lc0, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc1), read_lc1, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
+  attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
+
+  Serial.println("Load Cells Initialized");
+
+  Serial.print("Calibrating");
+  lc0.tare();
+  Serial.print(".");
+  lc1.tare();
+  Serial.print(".");
+  lc2.tare();
+  Serial.print(".");
+  lc3.tare();
+  Serial.println(".");
+  Serial.println("Load Cells Calibrated");
+}
+
+// ISRs to print data to serial monitor
+void read_lc0() { State.lc0 = lc0.get_value(); }
+void read_lc1() { State.lc1 = lc1.get_value(); }
+void read_lc2() { State.lc2 = lc2.get_value(); }
+void read_lc3() { State.lc3 = lc3.get_value(); }
 #endif
 
 #if defined(_WIN32) || defined(NATIVE)
+
+#include "native.h"
+outVector stateVector;
+
 int main() {
 
-  setup();
+  init_Vehicle(State);
+
+  // Determine when to burn
+  burnStartTimeCalc(State);
 
   do {
-    loop();
+    vehicleDynamics(State, PrevState);
+    thrustInfo(State);
+    pidController(State, PrevState);
+    TVC(State, PrevState);
+    processTVC(State);
+    state2vec(State, PrevState, stateVector);
+
+    // Set "prev" values for next timestep
+    PrevState = State;
+    State.time += State.stepSize;
+
+    if ((State.z < 0.0) && (State.thrustFiring == 2)) {
+      write2CSV(stateVector, State);
+      printSimResults(State);
+      init_Vehicle(State);
+    }
   } while ((State.z > 0.0) || (State.thrustFiring != 2));
 
   return 0;
 }
 #endif
-
-// ISRs to print data to serial monitor
-void read_lc0() { State.lc0 = lc0.get_value(); }
-
-void read_lc1() { State.lc1 = lc1.get_value(); }
-
-void read_lc2() { State.lc2 = lc2.get_value(); }
-
-void read_lc3() { State.lc3 = lc3.get_value(); }