From 82e43b1fe88b57901ae12fb59a8098ac5123d902 Mon Sep 17 00:00:00 2001
From: bpmcgeeney <bpmcgeeney@gmail.com>
Date: Sun, 14 Nov 2021 14:23:28 -0700
Subject: [PATCH] Added test gimbal function

---
 include/teensy.h | 35 +++++++++++++++++++++++++++++++++++
 src/main.cpp     |  9 +++++----
 2 files changed, 40 insertions(+), 4 deletions(-)

diff --git a/include/teensy.h b/include/teensy.h
index d3e2895..480711f 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -4,6 +4,7 @@
 #include <SD.h>
 #include <SPI.h>
 
+void testGimbal(class PWMServo &, class PWMServo &);
 double loadCellCalibrate();
 void initFile();
 void thrustInfo(struct Vehicle &);
@@ -15,6 +16,40 @@ void teensyAbort();
 const int chipSelect = BUILTIN_SDCARD;
 File dataFile;
 
+void testGimbal(PWMServo &servo1, PWMServo &servo2) {
+  int servoTest = 0;
+  ;
+
+  servo1.write(servoTest);
+  servo2.write(servoTest);
+
+  // Servo 1 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo1.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo1.write(servoTest);
+    delay(30);
+  }
+
+  delay(1000);
+
+  // Servo 2 Test
+  for (servoTest = 0; servoTest < 7; servoTest += 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+  for (servoTest = 7; servoTest >= 1; servoTest -= 1) {
+    servo2.write(servoTest);
+    delay(30);
+  }
+
+  delay(30);
+  servo1.write(0);
+  servo2.write(0);
+}
+
 void initFile() {
   Serial.print("Initializing SD card...");
 
diff --git a/src/main.cpp b/src/main.cpp
index 00105e0..47171ae 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -58,8 +58,8 @@ void setup() {
 
   delay(5000);
   Serial.println("Simulation Countdown:");
-  for (int i = 0; i < 15; i++) {
-    Serial.println(15 - i);
+  for (int i = 0; i < 10; i++) {
+    Serial.println(10 - i);
     delay(1000);
   }
 
@@ -72,9 +72,10 @@ void setup() {
 
   servo1.attach(pin_servo1);
   servo2.attach(pin_servo2);
-  servo1.write(0);
-  servo2.write(0);
+  testGimbal(servo1, servo2);
+  Serial.println("Servos Tested");
 
+  delay(1000);
   Serial.println("Simulated Vehicle Initalized");
 
   // Determine when to burn