From 8d53c7ea56998ea30372598e8d622a7240caa408 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Thu, 18 Nov 2021 12:19:11 -0700
Subject: [PATCH] enable servos in prep for verification

---
 src/main.cpp | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index e5ead20..9880511 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -14,8 +14,8 @@ float calc_height(float yaw, float pitch);
 
 void setup() {
   delay(5000);
-  // yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
-  // pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
+  yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
+  pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
 
   Serial.println("Level the TVC and press enter.");
   read_float();
@@ -26,7 +26,7 @@ void setup() {
     double pitch_pos = pct * PITCH_MAX;
 
     yaw.write(yaw_pos);
-    // pitch.write(pitch_pos);
+    pitch.write(pitch_pos);
     delay(15);
   }
 
@@ -38,19 +38,27 @@ void setup() {
 void loop() {
   Serial.print("Enter Yaw Coord: ");
   float yaw_pos = read_float();
+  if (abs(yaw_pos) > YAW_MAX) {
+    Serial.println("Value too large, defaulting to maximum deflection");
+    yaw_pos = YAW_MAX;
+  }
   Serial.println();
   Serial.println();
 
   Serial.print("Enter Pitch Coord: ");
   float pitch_pos = read_float();
+  if (abs(pitch_pos) > PITCH_MAX) {
+    Serial.println("Value too large, defaulting to maximum deflection");
+    pitch_pos = PITCH_MAX;
+  }
   Serial.println();
   Serial.println();
 
   Serial.println("Moving servos to [" + String(yaw_pos) + ", " +
                  String(pitch_pos) + "]");
 
-  // yaw.write(yaw_pos);
-  // pitch.write(pitch_pos);
+  yaw.write(yaw_pos);
+  pitch.write(pitch_pos);
 
   Serial.print("Calculating current height");
   for (int i = 0; i < 5; i++) {