From aec9a01e683c62fea27b00f5b6571608486f7069 Mon Sep 17 00:00:00 2001
From: Vynwg <me@vynwg.com>
Date: Thu, 30 Nov 2023 16:58:48 -0300
Subject: [PATCH] Yey

---
 platformio.ini |  2 +-
 src/servo.cpp  | 24 ++++++++++++++++++++++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/platformio.ini b/platformio.ini
index 30b2219..6ef56c1 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -17,4 +17,4 @@ lib_deps =
 	dfrobot/DFRobot_QMC5883@^1.0.0
 	juerd/ESP-WiFiSettings@^3.8.0
 	https://github.com/sivar2311/WebMonitor
-;upload_flags = --auth=senha1234
+upload_flags = --auth=senha1234
diff --git a/src/servo.cpp b/src/servo.cpp
index 9b59340..3979c6b 100644
--- a/src/servo.cpp
+++ b/src/servo.cpp
@@ -58,6 +58,30 @@ void ServoController::updateHeading() {
 }
 
 void ServoController::moveToAzAlt(float azimuth, float altitude) {
+    azimuth -= headingDegrees;
+
+    if (azimuth < 0) {
+        azimuth += 360;
+    }
+
+    if (altitude >= 0) {
+        if (azimuth <= 180) {
+            HrzSrv.write(azimuth);
+            delay(100);
+            VrtSrv.write(altitude);
+            delay(100);
+        } else {
+            HrzSrv.write(azimuth - 180);
+            delay(100);
+            VrtSrv.write(180 - altitude);
+            delay(100);
+        }
+    } else {
+        HrzSrv.write(0);
+        delay(100);
+        VrtSrv.write(90);
+        delay(100);
+    }
 
 }