Complete course done.

Firmware/micro_motors/micro_motors.ino

@@ -19,14 +19,14 @@ const char* password = "hi123456";
   #define FL_PWM 2
 
 // Motor BR (Back Right)
-  #define BR_IN1 33
+  #define BR_IN1 26         // Original: 33
-  #define BR_IN2 32
+  #define BR_IN2 25         // Original: 32
-  #define BR_PWM 23
+  #define BR_PWM 22         // Original: 23
 
   // Motor BL (Back Left)
-  #define BL_IN1 26
+  #define BL_IN1 33         // Original: 26
-  #define BL_IN2 25
+  #define BL_IN2 32         // Original: 25
-  #define BL_PWM 22
+  #define BL_PWM 23         // Original: 22
 
 
   // PWM settings
@@ -44,6 +44,7 @@ const char* password = "hi123456";
   int sens_LB = -1;
   int sens_RF = -1;
   int sens_RB = -1;
+  int sens_save = 10;
 
   // Trim values to calibrate wheel speeds
   int trimFL = 0;
@@ -90,8 +91,8 @@ void Go45right();
 void Go45left();
 void Go90right();
 void Go90left();
-void slideRight();
+void slideRight(int speed);
-void slideLeft();
+void slideLeft(int speed);
 void receiveSensorData();           // Receive the sensors data from micro sensors
 int rollingAverage(int buffer[]);   // rollingAverage function call
 void sensorsTest();
@@ -178,9 +179,9 @@ if (WiFi.status() == WL_CONNECTED) {
 }
 
 void loop() {
-    ph1 = true;
+//    ph1 = true;
 //    ph2 = true;
-    ph3 = true;
+//    ph3 = true;
 
 
     if (!ph1) {
@@ -199,7 +200,7 @@ void loop() {
     }
 
 //    motorsTest();
-    sensorsTest();
+//    sensorsTest();
 
 }
 
@@ -209,6 +210,11 @@ void phaseOne() {
         receiveSensorData();    // read sensors
         delay(10);
     }
+    delay(1000);
+    while (sens_FL < 20 || sens_FR < 20 || sens_FL > 8000 || sens_FR > 8000) {
+        receiveSensorData();    // read sensors
+        delay(10);
+    }
 
     int distance;
     int speed;
@@ -230,8 +236,6 @@ void phaseOne() {
 
     motorsStop();
 
-    align(245);
-
     logPrint("Stopped at: "); logPrintln((sens_FL + sens_FR) / 2);
     delay(1000);
     Go90right();
@@ -244,8 +248,8 @@ void phaseOne() {
     delay(1200);        // Go out of the pit 1800 miliseconds
     motorsStop();
     delay(500);
-    slideLeft();
+    slideLeft(150);
-    delay(1450);
+    delay(1350);        // Sliding time before going up the ramp
     motorsStop();
     delay(2000);
 }
@@ -257,56 +261,44 @@ void phaseTwo() {
         delay(10);
     }
 
-//    align(150);
-
-
     receiveSensorData();
 
-/*
-        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 < 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 > 500) {
-            motorsGo(-120);
-            delay(5);               // Wait for new sensor data to arrive
-            receiveSensorData();    // Get fresh reading each iteration
-        }
-*/
-
     delay(1000);
     motorsGo(-255);
-
     delay(3000);   
-
+        while ((sens_FL + sens_FR) / 2 > 320) {
-
-    
-        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();
-
+        delay(2000);
-
-
 }
 
 void phaseThree() {
 
-
+    slideRight(150);
+    delay(800);         // Slide right for 800 miliseconds
+    motorsStop();
+    delay(200);         // Very small delay between turning and going, optional
+    motorsGo(-100);
+    delay(1450);        // Go back towards the pin
+    motorsStop();
+    delay(200);         // Very small delay between turning and going, optional
+    Go45right();
+    motorsGo(-70);
+    delay(2400);
+    motorsStop();
+    delay(200);         // Very small delay between turning and going, optional
+    slideLeft(150);
+    delay(2500);
+    motorsStop();
+    delay(200);         // Very small delay between turning and going, optional
+    Go45left();
+    delay(200);
+    motorsGo(-200);
+    delay(1900);
+    motorsStop();
 }
 
 void otaTask(void *parameter) {
@@ -487,39 +479,6 @@ void receiveSensorData() {
               data += c;                                                                                                         
           }                                                                                                                      
       }  
-
-/*    
-      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;
-
-      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];
-      }
-*/
   }
 
 void sensorsTest() {
@@ -627,7 +586,7 @@ void Go45right() {
     motorFR(-150);
     motorBL(150);
     motorBR(-150);
-    delay(875);
+    delay(490);         // Change time here for angle adjustment
     motorsStop();
 }
 
@@ -636,7 +595,7 @@ void Go45left() {
     motorFR(150);
     motorBL(-150);
     motorBR(150);
-    delay(875);
+    delay(490);         // Change time here for angle adjustment
     motorsStop();
 }
 
@@ -645,7 +604,7 @@ void Go90right() {
     motorFR(-150);
     motorBL(150);
     motorBR(-150);
-    delay(1100);
+    delay(1100);        // Change time here for angle adjustment
     motorsStop();
 }
 
@@ -654,22 +613,22 @@ void Go90left() {
     motorFR(150);
     motorBL(-150);
     motorBR(150);
-    delay(1100);
+    delay(1100);        // Change time here for angle adjustment
     motorsStop();
 }
 
-void slideRight() {
+void slideRight(int speed) {
-    motorFL(-150);
+    motorFL(-speed);
-    motorFR(150);
+    motorFR(speed);
-    motorBL(150);
+    motorBL(speed);
-    motorBR(-150);
+    motorBR(-speed);
 }
 
-void slideLeft() {
+void slideLeft(int speed) {
-    motorFL(150);
+    motorFL(speed);
-    motorFR(-150);
+    motorFR(-speed);
-    motorBL(-150);
+    motorBL(-speed);
-    motorBR(150);
+    motorBR(speed);
 }
 
 void align(int dist) {