From 9a1ca56ac190b7774c23c78064d705c4612f757e Mon Sep 17 00:00:00 2001
From: Sam Hos <samhos2006@hotmail.com>
Date: Tue, 1 Apr 2025 09:46:35 +0200
Subject: [PATCH] cleanup and removed debug code

---
 src/controller/drone controller/src/main.cpp | 61 ++++++++++----------
 1 file changed, 31 insertions(+), 30 deletions(-)

diff --git a/src/controller/drone controller/src/main.cpp b/src/controller/drone controller/src/main.cpp
index 669187b..8904649 100644
--- a/src/controller/drone controller/src/main.cpp	
+++ b/src/controller/drone controller/src/main.cpp	
@@ -6,6 +6,7 @@
 int normalizePot(int pin, int minValue);
 int mapPot(int normalizedValue);
 int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin);
+void espNow();
 
 const int MAXPWMVALUE = 1000;
 const int MINPWMVALUE = 2000;
@@ -25,28 +26,7 @@ esp_now_peer_info_t peerInfo; // create a class object of the ESPNow class
 
 void setup()
 {
-  // Set device as a Wi-Fi Station
-  WiFi.mode(WIFI_STA);
-
-  // Init ESP-NOW
-  if (esp_now_init() != ESP_OK)
-  {
-    Serial.println("Error initializing ESP-NOW");
-    return;
-  }
-
-  // Register peer
-  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
-  peerInfo.channel = 0;
-  peerInfo.encrypt = false;
-
-  // Add peer
-  if (esp_now_add_peer(&peerInfo) != ESP_OK)
-  {
-    Serial.println("Failed to add peer");
-    return;
-  }
-
+  espNow();
   Serial.begin(9600);
 
   pinMode(D3, OUTPUT);
@@ -60,11 +40,13 @@ void setup()
 
 void loop()
 {
-  Serial.println(analogReadMultiPlexer(0, 0, 0, 0, A0)); // call normalizePot and put the output into mapPot then print it
+  Serial.print("X:" + String(myData.PWMCH2));
+  Serial.println(" Y:" + String(myData.PWMCH1));
+
 
   // Set values to send
   myData.PWMCH1 = mapPot(analogReadMultiPlexer(0, 0, 0, 0, A0));
-  myData.PWMCH2 = 1000; // test values
+  myData.PWMCH2 = mapPot(analogReadMultiPlexer(1, 0, 0, 0, A0)); // test values
   myData.PWMCH3 = 1000;
   myData.PWMCH4 = 1000;
 
@@ -78,7 +60,7 @@ void loop()
   {
     // Serial.println("Error sending the data");
   }
-  delay(10);
+  delay(10); //delay to avoid hammering the radio and to save power/heat
 }
 
 int mapPot(int normalizedValue)
@@ -103,9 +85,28 @@ int normalizePot(int pin, int minValue) // normalize the pot value to a range of
 int analogReadMultiPlexer(int addressA, int addressB, int addressC, int addressD, int pin)
 {
   digitalWrite(D3, LOW);
-  digitalWrite(D6, LOW);
-  digitalWrite(D7, LOW);
-  digitalWrite(D9, LOW);
-  digitalWrite(D8, LOW);
+  digitalWrite(D6, addressA);
+  digitalWrite(D7, addressB);
+  digitalWrite(D9, addressC);
+  digitalWrite(D8, addressD);
   return analogRead(pin);
-}
\ No newline at end of file
+}
+
+void espNow(){  // Set device as a Wi-Fi Station
+  WiFi.mode(WIFI_STA);
+  // Init ESP-NOW
+  if (esp_now_init() != ESP_OK)
+  {
+    Serial.println("Error initializing ESP-NOW");
+    return;
+  }
+  // Register peer
+  memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+  peerInfo.channel = 0;
+  peerInfo.encrypt = false;
+  // Add peer
+  if (esp_now_add_peer(&peerInfo) != ESP_OK)
+  {
+    Serial.println("Failed to add peer");
+    return;
+  }}