load cell integration

include/LoadCells.h

@@ -1,39 +0,0 @@
-
-#include <HX711.h>
-#pragma once
-
-struct LoadCells {
-  HX711 lc0;
-  HX711 lc1;
-  HX711 lc2;
-  HX711 lc3;
-
-  // Current calibrated value
-  double lc0Val;
-  double lc1Val;
-  double lc2Val;
-  double lc3Val;
-
-  // Calibration offset
-  double lc0Cal;
-  double lc1Cal;
-  double lc2Cal;
-  double lc3Cal;
-};
-
-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 update_LoadCells(LoadCells &loadCells) {
-  loadCells.lc0Val = loadCells.lc0.read() - loadCells.lc0Cal;
-  loadCells.lc1Val = loadCells.lc1.read() - loadCells.lc1Cal;
-  loadCells.lc2Val = loadCells.lc2.read() - loadCells.lc2Cal;
-  loadCells.lc3Val = loadCells.lc3.read() - loadCells.lc3Cal;
-}

include/Vehicle.h

@@ -44,6 +44,8 @@ struct Vehicle {
   double stepDuration;
 
   double time = 0.0;
+
+  double lc0, lc1, lc2, lc3;
 };
 
 void init_Vehicle(Vehicle &State) {

include/teensy.h

@@ -1,4 +1,3 @@
-#include "LoadCells.h"
 #include "Vehicle.h"
 #include <Arduino.h>
 #include <SD.h>
@@ -8,7 +7,7 @@ double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
 void processTVC(struct Vehicle &);
-void write2CSV(struct Vehicle &, double a, double b, double c, double d);
+void write2CSV(struct Vehicle &);
 void printSimResults(struct Vehicle &);
 void teensyAbort();
 
@@ -94,7 +93,7 @@ void thrustInfo(Vehicle &State) {
   }
 }
 
-void processTVC(Vehicle &State, LoadCells &loadCells) {
+void processTVC(Vehicle &State) {
   if (State.time == 0) {
     Serial.println("WARNING: processTVC not implemented for TEENSY");
   }
@@ -103,10 +102,14 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   // Vector math to aqcuire thrust vector components
   // PLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER
   // PLACEHOLDERPLACEHOLDER PLACEHOLDERPLACEHOLDER PLACEHOLDER
-  State.Fx = (loadCells.lc1Val - loadCells.lc2Val) * r / R;
-  State.Fy = (loadCells.lc0Val - loadCells.lc3Val) * r / R;
-  State.Fz =
-      loadCells.lc0Val + loadCells.lc1Val + loadCells.lc2Val + loadCells.lc3Val;
+  State.Fx = (State.lc1 - State.lc2) * r / R;
+  State.Fy = (State.lc0 - State.lc3) * r / R;
+  State.Fz = State.lc0 + State.lc1 + State.lc2 + State.lc3;
+
+  State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0));
+  State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0));
+  State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
+             (State.mass * g);
 
   // Calculate moment created by Fx and Fy
   State.momentX = State.Fx * State.momentArm;
@@ -116,7 +119,7 @@ void processTVC(Vehicle &State, LoadCells &loadCells) {
   State.F = sqrt(pow(State.Fx, 2) + pow(State.Fy, 2) + pow(State.Fz, 2));
 }
 
-void write2CSV(Vehicle &State, LoadCells &loadCells) {
+void write2CSV(Vehicle &State) {
   dataFile.print(String(State.time, 5));
   dataFile.print(",");
 
@@ -168,13 +171,13 @@ void write2CSV(Vehicle &State, LoadCells &loadCells) {
   dataFile.print(String(State.stepDuration, 5));
   dataFile.print(",");
 
-  dataFile.print(String(loadCells.lc0Val, 5));
+  dataFile.print(String(State.lc0, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc1Val, 5));
+  dataFile.print(String(State.lc1, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc2Val, 5));
+  dataFile.print(String(State.lc2, 5));
   dataFile.print(",");
-  dataFile.print(String(loadCells.lc3Val, 5));
+  dataFile.print(String(State.lc3, 5));
 
   dataFile.print("\n");
 }

src/main.cpp

@@ -23,10 +23,8 @@ unsigned long last, initTime;
 outVector stateVector;
 #elif defined(TEENSY)
 
-#include "LoadCells.h"
-LoadCells loadCells;
-
 #include "teensy.h"
+#include <HX711.h>
 const int lc_clock = 23;
 
 const int lc_data_0 = 14;
@@ -34,6 +32,16 @@ const int lc_data_1 = 15;
 const int lc_data_2 = 19;
 const int lc_data_3 = 20;
 
+void read_lc0();
+void read_lc1();
+void read_lc2();
+void read_lc3();
+
+HX711 lc0;
+HX711 lc1;
+HX711 lc2;
+HX711 lc3;
+
 #endif
 
 Vehicle State;
@@ -58,20 +66,37 @@ 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);
+  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
+  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");
 
-  loadCells.lc0Cal = loadCellCalibrate(loadCells.lc0);
-  loadCells.lc1Cal = loadCellCalibrate(loadCells.lc1);
-  loadCells.lc2Cal = loadCellCalibrate(loadCells.lc2);
-  loadCells.lc3Cal = loadCellCalibrate(loadCells.lc3);
+  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);
 
@@ -80,8 +105,6 @@ void setup() {
   Serial.println("Starting Height Calculated");
   delay(1000);
 
-  Serial.println("Load Cells Calibrated");
-  delay(1000);
   initFile();
   delay(1000);
 
@@ -112,16 +135,17 @@ void loop() {
 void loop() {
 
   last = micros();
-  update_LoadCells(loadCells);
+  Serial.println(State.time);
   vehicleDynamics(State, PrevState);
   thrustInfo(State);
   pidController(State, PrevState);
   TVC(State, PrevState);
 
-  processTVC(State, loadCells);
+  processTVC(State);
+  delay(2);
 
   State.stepDuration = micros() - last;
-  write2CSV(State, loadCells);
+  write2CSV(State);
 
   // Set "prev" values for next timestep
   PrevState = State;
@@ -156,4 +180,13 @@ int main() {
 
   return 0;
 }
-#endif
+#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(); }