Merge branch '22-make-function-to-turn-test-stand-load-cells-into-thrust-vector' into 'main'

Resolve "Make function to turn test stand load cells into thrust vector"

Closes #22

See merge request lander-team/lander-cpp!17

include/LoadCells.h

@@ -0,0 +1,37 @@
+
+#include <HX711.h>
+
+struct LoadCells {
+  HX711 loadcell_0;
+  HX711 loadcell_1;
+  HX711 loadcell_2;
+  HX711 loadcell_3;
+
+  double lc0Value;
+  double lc1Value;
+  double lc2Value;
+  double lc3Value;
+
+  double lc0Calibration;
+  double lc1Calibration;
+  double lc2Calibration;
+  double lc3Calibration;
+};
+
+double loadCellCalibrate(HX711 loadCell) {
+  // place code to calibrate load cells in here
+  double loadTotal = 0.0;
+  for (int t = 0; t == 10; ++t) {
+    loadTotal += loadCell.read();
+    delay(15);
+  }
+  return loadTotal / 10.0;
+}
+
+void init_LoadCells(LoadCells &loadCells) {
+
+  loadCells.lc0Calibration = loadCellCalibrate(loadCells.loadcell_0);
+  loadCells.lc1Calibration = loadCellCalibrate(loadCells.loadcell_1);
+  loadCells.lc2Calibration = loadCellCalibrate(loadCells.loadcell_2);
+  loadCells.lc3Calibration = loadCellCalibrate(loadCells.loadcell_3);
+}

include/teensy.h

@@ -16,7 +16,7 @@ File dataFile;
 
 double loadCellCalibrate() {
   // place code to calibrate load cells in here
-  double loadTotal;
+  double loadTotal = 0.0;
   for (double t = 0.0; t == 10.0; ++t) {
     loadTotal += 1.0;
     delay(15);
@@ -103,14 +103,16 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State) {
+void processTVC(Vehicle &State, LoadCells &loadCells) {
-  if (State.time == 0.0) {
+  if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
 
   // Vector math to aqcuire thrust vector components
-  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
+  // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
-  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
+  // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
+  State.Fx = loadCells.lc1Value + loadCells.lc2Value;
+  State.Fy = loadCells.lc0Value + loadCells.lc3Value;
   State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
              (State.mass * g);
 

platformio.ini

@@ -8,7 +8,6 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-
 [env:NATIVE]
 platform = native
 build_flags = -std=c++11 -Wall -O1 -D NATIVE
@@ -18,3 +17,4 @@ platform = teensy
 board = teensy41
 framework = arduino
 build_flags = -std=c++11 -Wall -O1 -D TEENSY
+lib_deps = bogde/HX711@^0.7.4

src/main.cpp

@@ -22,7 +22,18 @@ unsigned long last;
 #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();
@@ -81,11 +105,8 @@ void loop() {
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
-  processTVC(State);
+  processTVC(State, loadCells);
-  write2CSV(State);
+  // state2vec(State, PrevState, stateVector);
-
-  // Set "prev" values for next timestep
-  PrevState = State;
 
   State.time += State.stepSize;