Merge branch '31-avionics-integration-test' of gitlab.com:lander-team/lander-cpp into 31-avionics-integration-test

src/main.cpp

@@ -22,7 +22,18 @@ unsigned long last, initTime;
 #include "native.h"
 outVector stateVector;
 #elif defined(TEENSY)
+
+#include "LoadCells.h"
+LoadCells loadCells;
+
 #include "teensy.h"
+const int lc_clock = 23;
+
+const int lc_data_0 = 0;
+const int lc_data_1 = 1;
+const int lc_data_2 = 2;
+const int lc_data_3 = 3;
+
 #endif
 
 Vehicle State;
@@ -37,7 +48,18 @@ void setup() {
 }
 #elif defined(TEENSY)
 void setup() {
-  delay(5000);
+  Serial.begin(9600);
+
+  // 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(lc_data_0, INPUT);
+  pinMode(lc_data_1, INPUT);
+  pinMode(lc_data_2, INPUT);
+  pinMode(lc_data_3, INPUT);
+
+  delay(1000);
   init_Vehicle(State);
   Serial.println("Simulated Vehicle Initalized");
   delay(1000);
@@ -46,7 +68,9 @@ void setup() {
   burnStartTimeCalc(State);
   Serial.println("Starting Height Calculated");
   delay(1000);
-  loadCellCalibrate();
+
+  init_LoadCells(loadCells);
+
   Serial.println("Load Cells Calibrated");
   delay(1000);
   initFile();
@@ -83,17 +107,19 @@ void loop() {
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-  processTVC(State);
+  
+  processTVC(State, loadCells);
 
   State.stepDuration = micros() - last;
   write2CSV(State);
 
   // Set "prev" values for next timestep
   PrevState = State;
+  
+  // state2vec(State, PrevState, stateVector);
   State.time += State.stepSize;
 
   if ((State.z < 0.0) && (State.thrustFiring == 2)) {
-    printSimResults(State);
     Serial.println("Run duration:" + String(micros() - initTime) + " us");
 
     closeFile();