Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
sahilmgandhi
Date:
Sun May 28 06:48:16 2017 +0000
Parent:
40:465d2b565977
Child:
42:75257e6c4c76
Commit message:
fixed floodfill

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
main.h Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Sun May 28 06:22:34 2017 +0000
+++ b/main.cpp	Sun May 28 06:48:16 2017 +0000
@@ -160,11 +160,11 @@
         receiverThreeReading = IRP_3.getSamples(100);
         receiverFourReading = IRP_4.getSamples(100);
 
-        serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
-        serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
+        // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
+        // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
 
         if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
-            serial.printf("Front wall is there\n");
+            // serial.printf("Front wall is there\n");
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
             } else if (currDir % 4 == 1) {
@@ -178,7 +178,7 @@
         if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
             // do nothing, the walls are not there
         } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
-            serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
+            // serial.printf("Left wall, mouseY: %d, mouseX: %d, currDir = %d\n", mouseY, mouseX, currDir%4);
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
             } else if (currDir % 4 == 1) {
@@ -189,7 +189,7 @@
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
             }
         } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
-            serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
+            // serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
             } else if (currDir % 4 == 1) {
@@ -472,11 +472,11 @@
         receiverThreeReading = IRP_3.getSamples(100);
         receiverFourReading = IRP_4.getSamples(100);
 
-        serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
-        serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
+        // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
+        // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
 
         if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
-            serial.printf("Front wall is there\n");
+            // serial.printf("Front wall is there\n");
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
             } else if (currDir % 4 == 1) {
@@ -490,7 +490,7 @@
         if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
             // do nothing, the walls are not there
         } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
-            serial.printf("Left wall\n");
+            // serial.printf("Left wall\n");
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
             } else if (currDir % 4 == 1) {
@@ -501,7 +501,7 @@
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
             }
         } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
-            serial.printf("Right wall\n");
+            // serial.printf("Right wall\n");
             if (currDir % 4 == 0) {
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
             } else if (currDir % 4 == 1) {
@@ -512,7 +512,7 @@
                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
             }
         } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
-            serial.printf("Both walls \n");
+            // serial.printf("Both walls \n");
             if (currDir %4 == 0){
                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
@@ -532,19 +532,19 @@
 
 bool isWallInFront(int x, int y)
 {
-    return (wallArray[MAZE_LEN - y - 1][x]  & F_WALL);
+    return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
 }
 bool isWallInBack(int x, int y)
 {
-    return (wallArray[MAZE_LEN - y - 1][x]  & B_WALL);
+    return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
 }
 bool isWallOnRight(int x, int y)
 {
-    return (wallArray[MAZE_LEN - y - 1][x]  & R_WALL);
+    return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
 }
 bool isWallOnLeft(int x, int y)
 {
-    return (wallArray[MAZE_LEN - y - 1][x]  & L_WALL);
+    return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
 }
 
 int chooseNextMovement()
@@ -680,76 +680,76 @@
                 }
             }
         } else if (currDir % 4 == 2) {
-            if (mouseX - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
+            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
                     dirToGo = 1;
                 }
             }
-            if (mouseX + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
+            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
                     dirToGo = 2;
                 }
             }
-            if (mouseY - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
+            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                     dirToGo = 3;
                 }
             }
-            if (mouseY + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
+            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                     dirToGo = 4;
                 }
             }
         } else if (currDir % 4 == 1) {
-            if (mouseY - 1 >= 0 && !isWallOnRight(mouseX, mouseY)) {
+            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                     dirToGo = 1;
                 }
             }
-            if (mouseY + 1 < MAZE_LEN && !isWallOnLeft(mouseX, mouseY)) {
+            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
-                    serial.printf("Looks like the left wall was not there\n");
+                    // serial.printf("Looks like the left wall was not there\n");
                     dirToGo = 2;
                 }
             }
-            if (mouseX + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
+            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                     dirToGo = 3;
                 }
             }
-            if (mouseX - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
+            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                     dirToGo = 4;
                 }
             }
         } else if (currDir % 4 == 3) {
-            if (mouseY + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
+            if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
                     dirToGo = 1;
                 }
             }
-            if (mouseY - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
+            if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
                     dirToGo = 2;
                 }
             }
-            if (mouseX - 1 >= 0 && !isWallInFront(mouseX, mouseY)) {
+            if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
                     dirToGo = 3;
                 }
             }
-            if (mouseX + 1 < MAZE_LEN && !isWallInBack(mouseX, mouseY)) {
+            if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
                     dirToGo = 4;
--- a/main.h	Sun May 28 06:22:34 2017 +0000
+++ b/main.h	Sun May 28 06:48:16 2017 +0000
@@ -173,12 +173,12 @@
 //#define ID_CONSTANT 7.55
 
 const int desiredCount180 = 2900;   // change accordingly to the terrain
-const int desiredCountR = 1490;
-const int desiredCountL = 1500;
+const int desiredCountR = 1475;
+const int desiredCountL = 1495;
 
 const int oneCellCount = 5400;
 const int oneCellCountMomentum = 4590;//4570 (.15) speed;//4800;      // one cell count is actually approximately 5400, but this value is considering momentum!
-// const int oneCellCountMomentum = 4400;
+
 double receiverOneReading = 0.0;
 double receiverTwoReading = 0.0;
 double receiverThreeReading = 0.0;