From fc987e89ccc2aa53bbb8f835865adbe269c1a107 Mon Sep 17 00:00:00 2001
From: Anson Biggs <anson@ansonbiggs.com>
Date: Thu, 18 Nov 2021 12:15:41 -0700
Subject: [PATCH] terminal input working

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

diff --git a/src/main.cpp b/src/main.cpp
index bef12cc..e5ead20 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -10,13 +10,14 @@ double PITCH_MAX = 7;
 double zero_height;
 
 float read_float();
+float calc_height(float yaw, float pitch);
 
 void setup() {
-  delay(1000);
+  delay(5000);
   // yaw.attach(9);    // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
   // pitch.attach(10); // CHANGE MEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEEE
 
-  Serial.println("Level the TVC and press 1.");
+  Serial.println("Level the TVC and press enter.");
   read_float();
 
   // Do a full range of movement of the servos
@@ -30,19 +31,23 @@ void setup() {
   }
 
   Serial.print("Please input the point height in millimeters: ");
-  String zero_string = read_float();
+  zero_height = read_float();
+  delay(1000);
 }
 
 void loop() {
   Serial.print("Enter Yaw Coord: ");
   float yaw_pos = read_float();
-
   Serial.println();
+  Serial.println();
+
   Serial.print("Enter Pitch Coord: ");
   float pitch_pos = read_float();
   Serial.println();
+  Serial.println();
 
-  Serial.println("Moving servos...");
+  Serial.println("Moving servos to [" + String(yaw_pos) + ", " +
+                 String(pitch_pos) + "]");
 
   // yaw.write(yaw_pos);
   // pitch.write(pitch_pos);
@@ -57,14 +62,23 @@ void loop() {
   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");
+                 "\n");
+  Serial.println("Press enter to continue.");
+  read_float();
 }
 
 float read_float() {
-  while (Serial.available() == 0) {
-    delay(100);
+  char line[80];
+  int count = 0;
+
+  while (1) {
+    if (Serial.available() > 0) {
+      line[count] = (char)Serial.read(); // store the char
+      Serial.print(line[count]);
+      if (line[count++] == '\r') { // if its a CR,
+        line[count] = '\0';        //  zero-terminate it
+        return String(line).toFloat();
+      }
+    }
   }
-  float num = Serial.parseFloat();
-  Serial.print(num);
-  return num;
-}
\ No newline at end of file
+}