From bb6e2fdde3df4d319d2e6464f4e50baafded98a3 Mon Sep 17 00:00:00 2001
From: Ricardo <ricardo@electropepper.org>
Date: Thu, 5 Feb 2026 18:05:07 +0000
Subject: [PATCH] Working on end of phase 2

---
 Firmware/micro_motors/micro_motors.ino | 115 ++++++++++++++++++++-----
 1 file changed, 94 insertions(+), 21 deletions(-)

diff --git a/Firmware/micro_motors/micro_motors.ino b/Firmware/micro_motors/micro_motors.ino
index 56f5e07..4053156 100644
--- a/Firmware/micro_motors/micro_motors.ino
+++ b/Firmware/micro_motors/micro_motors.ino
@@ -97,6 +97,7 @@ int rollingAverage(int buffer[]);   // rollingAverage function call
 void sensorsTest();
 void motorsTest();
 void phaseThree();
+void align(int dist);
 
 void setup() {
   Serial.begin(115200);
@@ -177,7 +178,7 @@ if (WiFi.status() == WL_CONNECTED) {
 }
 
 void loop() {
-//    ph1 = true;
+    ph1 = true;
 //    ph2 = true;
     ph3 = true;
 
@@ -198,13 +199,13 @@ void loop() {
     }
 
 //    motorsTest();
-//    sensorsTest();
+    sensorsTest();
 
 }
 
 void phaseOne() {
     // Wait for valid sensor readings
-    while (sens_FL < 0 || sens_FR < 0) {
+    while (sens_FL < 20 || sens_FR < 20 || sens_FL > 8000 || sens_FR > 8000) {
         receiveSensorData();    // read sensors
         delay(10);
     }
@@ -213,11 +214,11 @@ void phaseOne() {
     int speed;
 
     receiveSensorData();
-    while ((distance = (sens_FL + sens_FR) / 2) > 240) {
+    while ((distance = (sens_FL + sens_FR) / 2) > 225) {
         // Slow down as we approach target
         if (distance > 500) {
-            speed = 180;
-        } else if (distance > 350) {
+            speed = 120;
+        } else if (distance > 450) {
             speed = 80;
         } else {
             speed = 40;         // Slow down even more
@@ -228,6 +229,9 @@ void phaseOne() {
     }
 
     motorsStop();
+
+    align(245);
+
     logPrint("Stopped at: "); logPrintln((sens_FL + sens_FR) / 2);
     delay(1000);
     Go90right();
@@ -248,33 +252,52 @@ void phaseOne() {
 
 void phaseTwo() {
     // Wait for valid sensor readings - ENABLE IF YOU ARE TESTING ONLY THIS PHASE
-//    while (sens_FL < 0 || sens_FR < 0) {
-//        receiveSensorData();    // read sensors
-//        delay(10);
-//    }
+    while (sens_FL < 0 || sens_FR < 0) {
+        receiveSensorData();    // read sensors
+        delay(10);
+    }
+
+//    align(150);
+
 
     receiveSensorData();
 
-
-//    while (!p2_stop) {
-        while ((sens_FL + sens_FR) / 2 > 8000) {
+/*
+        while ((sens_FL + sens_FR) / 2 > 7000) {
             motorsGo(-230);
             delay(5);               // Wait for new sensor data to arrive
             receiveSensorData();    // Get fresh reading each iteration
         }
-        while ((sens_FL + sens_FR) / 2 < 800) {     // Used to be 680
+        while ((sens_FL + sens_FR) / 2 < 300) {     // Used to be 680
             motorsGo(-250);
             delay(5);               // Wait for new sensor data to arrive
             receiveSensorData();    // Get fresh reading each iteration
         }
-        while ((sens_FL + sens_FR) / 2 > 320) {
+        while ((sens_FL + sens_FR) / 2 > 500) {
             motorsGo(-120);
             delay(5);               // Wait for new sensor data to arrive
             receiveSensorData();    // Get fresh reading each iteration
         }
-//        p2_stop = true;
-//        receiveSensorData();
-//    }
+*/
+
+    delay(1000);
+    motorsGo(-255);
+
+    delay(3000);
+
+
+    
+        while ((sens_FL + sens_FR) / 2 > 300) {
+            motorsGo(-100);
+            delay(5);               // Wait for new sensor data to arrive
+            receiveSensorData();    // Get fresh reading each iteration
+        }
+        while ((sens_FL + sens_FR) / 2 > 170) {
+            motorsGo(-80);
+            delay(5);               // Wait for new sensor data to arrive
+            receiveSensorData();    // Get fresh reading each iteration
+        }
+
         motorsStop();
 
 
@@ -430,7 +453,42 @@ void motorBR(int speed) {
     return sum / count;
 }
 
-  void receiveSensorData() {
+void receiveSensorData() {
+
+    static String data = "";                                                                                                   
+                                                                                                                                 
+      while (Serial2.available()) {                                                                                              
+          char c = Serial2.read();                                                                                               
+          if (c == '\n') {                                                                                                       
+              // Process complete line                                                                                           
+              int values[8];                                                                                                     
+              int idx = 0;                                                                                                       
+              int start = 0;                                                                                                     
+                                                                                                                                 
+              for (int i = 0; i <= data.length() && idx < 8; i++) {                                                              
+                  if (i == data.length() || data[i] == ',') {                                                                    
+                      values[idx++] = data.substring(start, i).toInt();                                                          
+                      start = i + 1;                                                                                             
+                  }                                                                                                              
+              }                                                                                                                  
+                                                                                                                                 
+              if (idx == 8) {                                                                                                    
+                  sens_FL = values[0];                                                                                           
+                  sens_FR = values[1];                                                                                           
+                  sens_BL = values[2];                                                                                           
+                  sens_BR = values[3];                                                                                           
+                  sens_LF = values[4];                                                                                           
+                  sens_LB = values[5];                                                                                           
+                  sens_RF = values[6];                                                                                           
+                  sens_RB = values[7];                                                                                           
+              }                                                                                                                  
+              data = "";                                                                                                         
+          } else if (c != '\r') {                                                                                                
+              data += c;                                                                                                         
+          }                                                                                                                      
+      }  
+
+/*    
       String data;
 
       // Drain buffer - keep only the latest complete line
@@ -461,6 +519,7 @@ void motorBR(int speed) {
           sens_RF = values[6];
           sens_RB = values[7];
       }
+*/
   }
 
 void sensorsTest() {
@@ -586,7 +645,7 @@ void Go90right() {
     motorFR(-150);
     motorBL(150);
     motorBR(-150);
-    delay(1250);
+    delay(1100);
     motorsStop();
 }
 
@@ -595,7 +654,7 @@ void Go90left() {
     motorFR(150);
     motorBL(-150);
     motorBR(150);
-    delay(1250);
+    delay(1100);
     motorsStop();
 }
 
@@ -612,3 +671,17 @@ void slideLeft() {
     motorBL(-150);
     motorBR(150);
 }
+
+void align(int dist) {                                                                                                         
+      receiveSensorData();                                                                                                       
+      while ((sens_FL + sens_FR) / 2 > dist) {                                                                                   
+          if (sens_FL > sens_FR) {                                                                                               
+              motorFL(50);                                                                                                       
+          } else {                                                                                                               
+              motorFR(50);                                                                                                       
+          }                                                                                                                      
+          delay(10);                                                                                                             
+          motorsStop();                                                                                                          
+          receiveSensorData();                                                                                                   
+      }                                                                                                                          
+  }
\ No newline at end of file