From 4fee260ed2f1c48dd1b7881fe716eb022000c56f Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Wed, 3 Nov 2021 15:47:08 -0700
Subject: [PATCH] started outlining test

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

diff --git a/src/main.cpp b/src/main.cpp
index ac1b478..e052a40 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -4,20 +4,33 @@
 Servo yaw;
 Servo pitch;
 
+double YAW_MAX = 7;
+double PITCH_MAX = 7;
+
+double zero_height;
+
+double point_height(double TVC_angle) { return sin(TVC_angle) + zero_height; }
+
 void setup() {
-  yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
+  yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
   pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
+
+  Serial.println("Level the TVC and press enter.");
+  Serial.readString();
+
+  // Do a full range of movement of the servos
+  for (double pct = -100.0; pct <= 100.0; pct += 1.0) {
+    double yaw_pos = pct * YAW_MAX;
+    double pitch_pos = pct * PITCH_MAX;
+
+    yaw.write(yaw_pos);
+    pitch.write(pitch_pos);
+    delay(15);
+  }
+
+  Serial.print("Please input the point height in millimeters: ");
+  String zero_string = Serial.readString();
+  zero_height = zero_string.toFloat();
 }
 
-void loop() {
-  for (int pos = 0; pos <= 180; pos += 1) {
-    yaw.write(pos);
-    pitch.write(pos);
-    delay(15);
-  }
-  for (int pos = 180; pos >= 0; pos -= 1) {
-    yaw.write(pos);
-    pitch.write(pos);
-    delay(15);
-  }
-}
\ No newline at end of file
+void loop() {}
\ No newline at end of file