From 71170e8985cf551a79653b0b67b43a6af752cd5e Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Mon, 22 Nov 2021 14:04:10 -0700
Subject: [PATCH] fixed servo center and initialized both lc and lc_processed

---
 .vscode/settings.json |  3 ++-
 include/teensy.h      | 29 +++++++++++++++++------------
 src/main.cpp          |  2 --
 3 files changed, 19 insertions(+), 15 deletions(-)

diff --git a/.vscode/settings.json b/.vscode/settings.json
index cbe555d..56857d0 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -46,7 +46,8 @@
         "chrono": "cpp",
         "ratio": "cpp",
         "thread": "cpp",
-        "string_view": "cpp"
+        "string_view": "cpp",
+        "*.tcc": "cpp"
     },
     "C_Cpp.clang_format_fallbackStyle": "LLVM",
     "editor.formatOnSave": true,
diff --git a/include/teensy.h b/include/teensy.h
index e5a1c2e..4685e13 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -52,11 +52,11 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
 
   // Servo 1 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    yaw.write(yaw_conv(90 + servoTest));
+    yaw.write(yaw_conv(servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    yaw.write(yaw_conv(90 + servoTest));
+    yaw.write(yaw_conv(servoTest));
     delay(30);
   }
 
@@ -64,11 +64,11 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
 
   // Servo 2 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    pitch.write(pitch_conv(90 + servoTest));
+    pitch.write(pitch_conv(servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    pitch.write(pitch_conv(90 + servoTest));
+    pitch.write(pitch_conv(servoTest));
     delay(30);
   }
 
@@ -76,13 +76,13 @@ void testGimbal(PWMServo &yaw, PWMServo &pitch) {
 
   // Servo 1 & 2 Test
   for (servoTest = 0; servoTest < 7; servoTest += 1) {
-    yaw.write(yaw_conv(90 + servoTest));
-    pitch.write(pitch_conv(90 + servoTest));
+    yaw.write(yaw_conv(servoTest));
+    pitch.write(pitch_conv(servoTest));
     delay(30);
   }
   for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
-    yaw.write(yaw_conv(90 + servoTest));
-    pitch.write(pitch_conv(90 + servoTest));
+    yaw.write(yaw_conv(servoTest));
+    pitch.write(pitch_conv(servoTest));
     delay(30);
   }
 
@@ -130,10 +130,15 @@ void initLoadCells(Vehicle &State) {
   Serial.println("Load Cells Initialized");
 
   // Initializes State variables
-  State.lc0 = 9.81 * 0.001 * lc0.get_value() / lcGain0;
-  State.lc1 = 9.81 * 0.001 * lc1.get_value() / lcGain1;
-  State.lc2 = 9.81 * 0.001 * lc2.get_value() / lcGain2;
-  State.lc3 = 9.81 * 0.001 * lc3.get_value() / lcGain3;
+  State.lc0 = lc0.get_value();
+  State.lc1 = lc1.get_value();
+  State.lc2 = lc2.get_value();
+  State.lc3 = lc3.get_value();
+
+  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;
+  State.lc3_processed = 9.81 * 0.001 * lc3.get_value() / lcGain3;
 }
 
 void initFile() {
diff --git a/src/main.cpp b/src/main.cpp
index 99562f6..379f5da 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -57,8 +57,6 @@ void setup() {
 
   initFile();
 
-  // PLACE BUTTON HERE---------------------------------------------
-
   initTime = micros();
 }