PhaseTwo completed.

Firmware/micro_motors/micro_motors.ino

@@ -50,9 +50,22 @@ const char* password = "hi123456";
   int trimFR = 0;                                                                                                                
   int trimBL = 0;                                                                                                                
   int trimBR = 0;
+    #define BUFFER_SIZE 7
+    int buffer_FL[BUFFER_SIZE];
+    int buffer_FR[BUFFER_SIZE];
+    int buffer_BL[BUFFER_SIZE];
+    int buffer_BR[BUFFER_SIZE];
+    int buffer_LF[BUFFER_SIZE];
+    int buffer_LB[BUFFER_SIZE];
+    int buffer_RF[BUFFER_SIZE];
+    int buffer_RB[BUFFER_SIZE];
 
+    int bufferIndex = 0;
+    bool bufferFilled = false;
     bool ph1 = false;           // Phase 1 complete flag 
-
+    bool ph2 = false;           //phase 2 complete flag
+    bool ph3 = false;           //phase 3 complete flag
+    bool p2_stop = false;       // Flag when its passed the top of the hill
 
 // Telnet server to watch serial
 WiFiServer telnetServer(23);                                                                                                   
@@ -80,9 +93,10 @@ void Go90left();
 void slideRight();
 void slideLeft();
 void receiveSensorData();           // Receive the sensors data from micro sensors
+int rollingAverage(int buffer[]);   // rollingAverage function call
 void sensorsTest();
 void motorsTest();
-
+void phaseThree();
 
 void setup() {
   Serial.begin(115200);
@@ -164,14 +178,27 @@ if (WiFi.status() == WL_CONNECTED) {
 
 void loop() {
 //    ph1 = true;
+//    ph2 = true;
+//    ph3 = true;
+
 
     if (!ph1) {
         phaseOne();
         ph1 = true;
     }
     
+    if (!ph2) {
+        phaseTwo();
+        ph2 = true;
+    }
+    
+    if (!ph3) {
+        phaseThree();
+        ph3 = true;
+    }
+
 //    motorsTest();
-//    sensorsTest();
+    sensorsTest();
 
 }
 
@@ -186,14 +213,14 @@ void phaseOne() {
     int speed;
 
     receiveSensorData();
-    while ((distance = (sens_FL + sens_FR) / 2) > 250) {
+    while ((distance = (sens_FL + sens_FR) / 2) > 240) {
         // Slow down as we approach target
         if (distance > 500) {
             speed = 180;
-        } else if (distance > 400) {
-            speed = 120;
+        } else if (distance > 350) {
+            speed = 80;
         } else {
-            speed = 80;         // Slow down even more
+            speed = 40;         // Slow down even more
         }
         motorsGo(speed);
         delay(5);               // Wait for new sensor data to arrive
@@ -206,12 +233,57 @@ void phaseOne() {
     Go90right();
     delay(2000);
     motorsGo(80);
-    delay(1650);        // Go into the pit 1800 miliseconds
+    delay(1250);        // Go into the pit 1800 miliseconds,
     motorsStop();
-    delay(3000);
+    delay(3000);    //how long to pick balls? dunno
     motorsGo(-150);
-    delay(1600);        // Go out of the pit 1800 miliseconds
+    delay(1200);        // Go out of the pit 1800 miliseconds
     motorsStop(); 
+    delay(500);
+    slideLeft();
+    delay(1450);
+    motorsStop();
+    delay(2000);
+}
+
+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);
+//    }
+
+    receiveSensorData();
+
+
+    while (!p2_stop) {
+        while ((sens_FL + sens_FR) / 2 > 8000) {
+            motorsGo(-255);
+            delay(5);               // Wait for new sensor data to arrive
+            receiveSensorData();    // Get fresh reading each iteration
+        }
+        while ((sens_FL + sens_FR) / 2 < 680) {     // Used to be 680
+            motorsGo(-180);
+            delay(5);               // Wait for new sensor data to arrive
+            receiveSensorData();    // Get fresh reading each iteration
+        }
+        while ((sens_FL + sens_FR) / 2 > 320) {
+            motorsGo(-120);
+            delay(5);               // Wait for new sensor data to arrive
+            receiveSensorData();    // Get fresh reading each iteration
+        }
+        p2_stop = true;
+        receiveSensorData();
+    }
+        motorsStop();
+
+
+
+}
+
+void phaseThree() {
+
+
 }
 
 void otaTask(void *parameter) {                                                                                                
@@ -347,6 +419,17 @@ void motorBR(int speed) {
       motorBL(0);                                                                                            
   }
 
+  int rollingAverage(int buffer[]) {
+    int sum = 0;
+    int count = bufferFilled ? BUFFER_SIZE : (bufferIndex + 1);
+    
+    for (int i = 0; i < count; i++) {
+        sum += buffer[i];
+    }
+    
+    return sum / count;
+}
+
   void receiveSensorData() {
       String data;
 
@@ -503,7 +586,7 @@ void Go90right() {
     motorFR(-150);                                                                                                           
     motorBL(150);                                                                                                            
     motorBR(-150);
-    delay(1150);
+    delay(1250);
     motorsStop();
 }
 
@@ -512,7 +595,7 @@ void Go90left() {
     motorFR(150);                                                                                                           
     motorBL(-150);                                                                                                            
     motorBR(150);
-    delay(1150);
+    delay(1250);
     motorsStop();
 }