From 644fcc0931d3be73b736bf9c2ba2a4cc51d788c7 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Wed, 3 Nov 2021 20:29:10 -0700
Subject: [PATCH] hopefully this works on the teensy

---
 src/main.cpp | 28 +++++++++++++++++++++++++---
 1 file changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index e052a40..e3d15f6 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -9,8 +9,6 @@ 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 MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
   pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
@@ -33,4 +31,28 @@ void setup() {
   zero_height = zero_string.toFloat();
 }
 
-void loop() {}
\ No newline at end of file
+void loop() {
+  Serial.print("Enter Yaw Coord: ");
+  float yaw_pos = Serial.readString().toFloat();
+  Serial.println();
+  Serial.print("Enter Pitch Coord: ");
+  float pitch_pos = Serial.readString().toFloat();
+  Serial.println();
+
+  Serial.println("Moving servos...");
+
+  yaw.write(yaw_pos);
+  pitch.write(pitch_pos);
+
+  Serial.print("Calculating current height");
+  for (int i = 0; i < 5; i++) {
+    Serial.print(".");
+    delay(i * 100);
+  }
+  Serial.println("");
+
+  float height = sin(yaw_pos) + sin(pitch_pos) + zero_height;
+
+  Serial.println("Please verify current height is correct: " + String(height) +
+                 "\n\n\n\n\n");
+}
\ No newline at end of file