Working on Phase 1

Firmware/micro_motors/micro_motors.ino

@@ -51,6 +51,8 @@ const char* password = "hi123456";
   int trimBL = 0;                                                                                                                
   int trimBR = 0;
 
+bool ph1 = false;    // Phase 1 complete flag 
+
 
 // Telnet server to watch serial
 WiFiServer telnetServer(23);                                                                                                   
@@ -161,22 +163,56 @@ if (WiFi.status() == WL_CONNECTED) {
 }
 
 void loop() {
+//    ph1 = true;
 
-receiveSensorData(); 
-while ((sens_FL + sens_FR) / 2 > 174) {
-    motorsGo(180);
-    receiveSensorData(); 
+    if (!ph1) {
+        phaseOne();
+        ph1 = true;
     }
 
-motorsStop();
-delay(1000);
-Go90right();
-
 //    motorsTest();
 //    sensorsTest();
 
 }
 
+void phaseOne() {
+    // Wait for valid sensor readings
+    while (sens_FL < 0 || sens_FR < 0) {
+        receiveSensorData();    // read sensors
+        delay(10);
+    }
+
+    int distance;
+    int speed;
+
+    receiveSensorData();
+    while ((distance = (sens_FL + sens_FR) / 2) > 250) {
+        // Slow down as we approach target
+        if (distance > 500) {
+            speed = 180;
+        } else if (distance > 400) {
+            speed = 120;
+        } else {
+            speed = 80;         // Slow down even more
+        }
+        motorsGo(speed);
+        delay(5);               // Wait for new sensor data to arrive
+        receiveSensorData();    // Get fresh reading each iteration
+    }
+
+    motorsStop();
+    logPrint("Stopped at: "); logPrintln((sens_FL + sens_FR) / 2);
+    delay(1000);
+    Go90right();
+    delay(2000);
+    motorsGo(80);
+    delay(1650);        // Go into the pit 1800 miliseconds
+    motorsStop();
+    delay(3000);
+    motorsGo(-150);
+    delay(1600);        // Go out of the pit 1800 miliseconds
+    motorsStop(); 
+}
 
 void otaTask(void *parameter) {                                                                                                
       for (;;) {                                                                                                                 
@@ -312,8 +348,15 @@ void motorBR(int speed) {
   }
 
   void receiveSensorData() {
-      if (Serial2.available()) {                                                                             
-          String data = Serial2.readStringUntil('\n');                                                       
+      String data;
+
+      // Drain buffer - keep only the latest complete line
+      while (Serial2.available()) {
+          data = Serial2.readStringUntil('\n');
+      }
+
+      if (data.length() == 0) return;
+
       int values[8];
       int idx = 0;
       int start = 0;
@@ -336,7 +379,6 @@ void motorBR(int speed) {
           sens_RB = values[7];
       }                                                                                                      
   }
-  }
 
 void sensorsTest() {
 
@@ -461,7 +503,7 @@ void Go90right() {
     motorFR(-150);                                                                                                           
     motorBL(150);                                                                                                            
     motorBR(-150);
-    delay(1735);
+    delay(1150);
     motorsStop();
 }
 
@@ -470,7 +512,7 @@ void Go90left() {
     motorFR(150);                                                                                                           
     motorBL(-150);                                                                                                            
     motorBR(150);
-    delay(1735);
+    delay(1150);
     motorsStop();
 }