From a47cef4bc9b1c452f32725c05192a9284d6cb548 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Mon, 29 Nov 2021 10:42:48 -0700
Subject: [PATCH] added abort conditions for load cells and tvc

---
 include/teensy.h | 29 ++++++++++++++++++++++++++---
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index 69d2b55..9659c10 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -47,6 +47,11 @@ File dataFile;
 void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   int servoTest = 90;
 
+  if ((!yaw.attached()) || (!pitch.attached())) {
+    Serial.println("Servo pin assignments are not valid!");
+    teensyAbort();
+  }
+
   yaw.write(yaw_conv(servoTest));
   pitch.write(pitch_conv(servoTest));
 
@@ -87,7 +92,7 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
   }
 
   delay(30);
-  // BRING IN YAW AND PITCH CONVERSION FROM TVC TEST
+
   yaw.write(yaw_conv(90));
   pitch.write(pitch_conv(90));
 }
@@ -127,7 +132,23 @@ void initLoadCells(Vehicle &State) {
   attachInterrupt(digitalPinToInterrupt(pin_lc2), read_lc2, LOW);
   attachInterrupt(digitalPinToInterrupt(pin_lc3), read_lc3, LOW);
 
-  Serial.println("Load Cells Initialized");
+  Serial.println("Checking tare");
+  double lc0Avg, lc1Avg, lc2Avg, lc3Avg = 0.0;
+  double loopSize = 50.0;
+
+  for (int i = 0; i < loopSize; i++) {
+    lc0Avg += (State.lc0 / loopSize);
+    lc1Avg += (State.lc1 / loopSize);
+    lc2Avg += (State.lc2 / loopSize);
+    lc3Avg += (State.lc3 / loopSize);
+    delay(50);
+  }
+
+  if (((lc0Avg > 1000) || (lc1Avg > 1000)) ||
+      ((lc2Avg > 1000) || (lc3Avg > 1000))) {
+    Serial.println("Load cell tare insufficient!");
+    teensyAbort();
+  }
 
   // Initializes State variables
   State.lc0 = lc0.get_value();
@@ -135,6 +156,8 @@ void initLoadCells(Vehicle &State) {
   State.lc2 = lc2.get_value();
   State.lc3 = lc3.get_value();
 
+  Serial.println("Load Cells Initialized");
+
   State.lc0_processed = 9.81 * 0.001 * lc0.get_value() / lcGain0;
   State.lc1_processed = 9.81 * 0.001 * lc1.get_value() / lcGain1;
   State.lc2_processed = 9.81 * 0.001 * lc2.get_value() / lcGain2;
@@ -388,7 +411,6 @@ void read_lc3() {
       loadCellFilter(State.lc3_processed, PrevState.lc3_processed);
   PrevState.lc3_processed = State.lc3_processed;
 }
-
 double loadCellFilter(double current, double previous) {
   if (abs(current - previous > 60.0)) {
     return previous;
@@ -397,6 +419,7 @@ double loadCellFilter(double current, double previous) {
   }
 }
 
+// Desired TVC angle to servo input conversion
 float yaw_conv(float thetad) {
   float h = .2875;
   float H = 1.6;