From b9c2e7fb3557eef605fde82528f3955268cc0d77 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Sat, 13 Nov 2021 11:17:44 -0700
Subject: [PATCH] now reading from load cells each loop

---
 include/LoadCells.h | 37 +++++++++++++++++++------------------
 include/teensy.h    |  5 ++---
 src/main.cpp        | 18 ++++++++++++++----
 3 files changed, 35 insertions(+), 25 deletions(-)

diff --git a/include/LoadCells.h b/include/LoadCells.h
index 7aefd5d..2b89a93 100644
--- a/include/LoadCells.h
+++ b/include/LoadCells.h
@@ -2,20 +2,22 @@
 #include <HX711.h>
 
 struct LoadCells {
-  HX711 loadcell_0;
-  HX711 loadcell_1;
-  HX711 loadcell_2;
-  HX711 loadcell_3;
+  HX711 lc0;
+  HX711 lc1;
+  HX711 lc2;
+  HX711 lc3;
 
-  double lc0Value;
-  double lc1Value;
-  double lc2Value;
-  double lc3Value;
+  // Current calibrated value
+  double lc0Val;
+  double lc1Val;
+  double lc2Val;
+  double lc3Val;
 
-  double lc0Calibration;
-  double lc1Calibration;
-  double lc2Calibration;
-  double lc3Calibration;
+  // Calibration offset
+  double lc0Cal;
+  double lc1Cal;
+  double lc2Cal;
+  double lc3Cal;
 };
 
 double loadCellCalibrate(HX711 loadCell) {
@@ -28,10 +30,9 @@ double loadCellCalibrate(HX711 loadCell) {
   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);
+void update_LoadCells(LoadCells &loadCells) {
+  loadCells.lc0Val = loadCells.lc0.read();
+  loadCells.lc1Val = loadCells.lc1.read();
+  loadCells.lc2Val = loadCells.lc2.read();
+  loadCells.lc3Val = loadCells.lc3.read();
 }
\ No newline at end of file
diff --git a/include/teensy.h b/include/teensy.h
index f948298..b04d15c 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -14,7 +14,6 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
-
 void initFile() {
   Serial.print("Initializing SD card...");
 
@@ -102,8 +101,8 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = loadCells.lc1Value + loadCells.lc2Value;
-  State.Fy = loadCells.lc0Value + loadCells.lc3Value;
+  State.Fx = loadCells.lc1Val + loadCells.lc2Val;
+  State.Fy = loadCells.lc0Val + loadCells.lc3Val;
   State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
              (State.mass * g);
 
diff --git a/src/main.cpp b/src/main.cpp
index 5193263..5e9abd7 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -58,6 +58,17 @@ void setup() {
   pinMode(lc_data_1, INPUT);
   pinMode(lc_data_2, INPUT);
   pinMode(lc_data_3, INPUT);
+  loadCells.lc0.begin(lc_data_0, lc_clock);
+  loadCells.lc1.begin(lc_data_1, lc_clock);
+  loadCells.lc2.begin(lc_data_2, lc_clock);
+  loadCells.lc3.begin(lc_data_3, lc_clock);
+  Serial.println("Load Cells Initialized");
+
+  loadCells.lc0Cal = loadCellCalibrate(loadCells.lc0);
+  loadCells.lc1Cal = loadCellCalibrate(loadCells.lc1);
+  loadCells.lc2Cal = loadCellCalibrate(loadCells.lc2);
+  loadCells.lc3Cal = loadCellCalibrate(loadCells.lc3);
+  Serial.println("Load Cells Calibrated");
 
   delay(1000);
   init_Vehicle(State);
@@ -69,8 +80,6 @@ void setup() {
   Serial.println("Starting Height Calculated");
   delay(1000);
 
-  init_LoadCells(loadCells);
-
   Serial.println("Load Cells Calibrated");
   delay(1000);
   initFile();
@@ -103,6 +112,7 @@ void loop() {
 void loop() {
 
   last = micros();
+  update_LoadCells(loadCells);
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
@@ -111,8 +121,8 @@ void loop() {
   processTVC(State, loadCells);
 
   State.stepDuration = micros() - last;
-  write2CSV(State, loadCells.lc0Value, loadCells.lc1Value, loadCells.lc2Value,
-            loadCells.lc3Value);
+  write2CSV(State, loadCells.lc0Val, loadCells.lc1Val, loadCells.lc2Val,
+            loadCells.lc3Val);
 
   // Set "prev" values for next timestep
   PrevState = State;