Mouse code for the MacroRat

Dependencies:   ITG3200 QEI

Files at this revision

API Documentation at this revision

Comitter:
sahilmgandhi
Date:
Wed May 24 21:47:21 2017 +0000
Parent:
30:11f4316a5ba7
Child:
32:69acb14778ea
Commit message:
newly modified floodfill, need to get PID with new values now!

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	Wed May 24 02:40:36 2017 +0000
+++ b/main.cpp	Wed May 24 21:47:21 2017 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
- 
+
 #include "irpair.h"
 #include "main.h"
 #include "motor.h"
- 
+
 #include <stdlib.h>
 #include <stack>          // std::stack
 #include <utility>      // std::pair, std::make_pair
@@ -11,62 +11,61 @@
 #include "ITG3200.h"
 #include "stm32f4xx.h"
 #include "QEI.h"
- 
-
 
 //IRSAvg= >: 0.143701, 0.128285
- 
-        //facing wall ir2 and ir3
-        //0.144562, 0.113971 in maze
- 
-        // normal hall ir2 and ir3
-        //0.013665, 0.010889  in maze
- 
-        //0.014462, 0.009138 
-        // 0.013888, 0.010530 
- 
- 
-        //no walls ir2 and ir3
-        //0.003265, 0.002904  in maze9
- 
-        //0.003162, 0.003123 
-        //0.003795, 
- 
-//0.005297, 0.007064 
+
+//facing wall ir2 and ir3
+//0.144562, 0.113971 in maze
+
+// normal hall ir2 and ir3
+//0.013665, 0.010889  in maze
+
+//0.014462, 0.009138
+// 0.013888, 0.010530
+
+
+//no walls ir2 and ir3
+//0.003265, 0.002904  in maze9
+
+//0.003162, 0.003123
+//0.003795,
+
+//0.005297, 0.007064
 
 void pidOnEncoders();
 
-void moveForwardCellEncoder(double cellNum){
+void moveForwardCellEncoder(double cellNum)
+{
     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellNum;
     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellNum;
- 
+
     left_motor.forward(0.125);
     right_motor.forward(0.095);
     wait_ms(1);
-    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
         receiverTwoReading = IRP_2.getSamples(100);
         receiverThreeReading = IRP_3.getSamples(100);
         // serial.printf("Average 2: %f Average 3: %f Sensor 2: %f Sensor 3: %f\n", IRP_2.sensorAvg, IRP_3.sensorAvg, receiverTwoReading, receiverThreeReading);
-        if (receiverThreeReading < ir3base){
+        if (receiverThreeReading < ir3base) {
             // redLed.write(1);
             // blueLed.write(0);
             turnFlag |= RIGHT_FLAG;
-        }
-        else if (receiverTwoReading < ir2base){
+        } else if (receiverTwoReading < ir2base) {
             // redLed.write(0);
             // blueLed.write(1);
             turnFlag |= LEFT_FLAG;
         }
         pidOnEncoders();
     }
- 
+
     left_motor.brake();
     right_motor.brake();
 }
- 
- 
-void moveForwardEncoder(double num){
- 
+
+
+void moveForwardEncoder(double num)
+{
+
     int count0;
     int count1;
     count0 = encoder0.getPulses();
@@ -82,12 +81,11 @@
     double speed1 = 0.12;
     right_motor.move(speed0);
     left_motor.move(speed1);
- 
- 
-    while(  ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)  ||   IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8){
-    //while(     (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4   ){
+
+    while(  ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)*num)  ||   IRP_1.getSamples(50) > IRP_1.sensorAvg*0.8 || IRP_4.getSamples(50) > IRP_4.sensorAvg*0.8) {
+        //while(     (IRP_1.getSamples(50) + IRP_4.getSamples(50))/2 < ((IRP_1.sensorAvg+IRP_2.sensorAvg)/2)*0.4   ){
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-  
+
         count0 = encoder0.getPulses() - initial0;
         count1 = encoder1.getPulses() - initial1;
         int x = count0 - count1;
@@ -95,36 +93,34 @@
         double kppart = kp * x;
         double kdpart = kd * (x-prev);
         double d = kppart + kdpart;
-        
+
         //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
-        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
-        {
+        if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
             left_motor.move( speed1-0.8*d );
             right_motor.move( speed0+d );
-        }
-        else if( x > diff + 40 )
-        {
+        } else if( x > diff + 40 ) {
             left_motor.move( speed1-0.8*d );
             right_motor.move( speed0+d );
         }
         // else
         // {
         //     left_motor.brake();
-        //     right_motor.brake();   
+        //     right_motor.brake();
         // }
-        prev = x; 
+        prev = x;
     }
- 
+
     //pidOnEncoders();
     //pidBrake();
     right_motor.brake();
     left_motor.brake();
     return;
 }
- 
- 
-void moveForwardWallEncoder(){
- int count0;
+
+
+void moveForwardWallEncoder()
+{
+    int count0;
     int count1;
     count0 = encoder0.getPulses();
     count1 = encoder1.getPulses();
@@ -143,15 +139,15 @@
     float ir1 = IRP_1.getSamples(50);
     float ir4 = IRP_4.getSamples(50);
 
-    if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4){
+    if((ir1 + ir4)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4) {
         return;
     }
 
     //while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) {
     //while(     (ir1 + ir4)/2 < ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.4   ){
-    while(  ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ){
+    while(  ir1 < IRP_1.sensorAvg*0.7 || ir4 < IRP_4.sensorAvg*0.7 ) {
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-  
+
         count0 = encoder0.getPulses() - initial0;
         count1 = encoder1.getPulses() - initial1;
         int x = count0 - count1;
@@ -159,24 +155,21 @@
         double kppart = kp * x;
         double kdpart = kd * (x-prev);
         double d = kppart + kdpart;
-        
+
         //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
-        if( x < diff - 40 ) // count1 is bigger, right wheel pushed forward
-        {
+        if( x < diff - 40 ) { // count1 is bigger, right wheel pushed forward
             left_motor.move( speed1-0.8*d );
             right_motor.move( speed0+d );
-        }
-        else if( x > diff + 40 )
-        {
+        } else if( x > diff + 40 ) {
             left_motor.move( speed1-0.8*d );
             right_motor.move( speed0+d );
         }
         // else
         // {
         //     left_motor.brake();
-        //     right_motor.brake();   
+        //     right_motor.brake();
         // }
-        prev = x; 
+        prev = x;
         ir1 = IRP_1.getSamples(50);
         ir4 = IRP_4.getSamples(50);
     }
@@ -187,7 +180,7 @@
     left_motor.brake();
     return;
 }
- 
+
 void moveForwardUntilWallIr()
 {
     double currentError = 0;
@@ -205,16 +198,16 @@
 
     int count = encoder0.getPulses();
     while ((IRP_1.getSamples( SAMPLE_NUM ) + IRP_4.getSamples( SAMPLE_NUM ) )/2 < 0.05f) { // while the front facing IR's arent covered
-        
+
         if((IRP_2.getSamples(SAMPLE_NUM) < 0.005 || IRP_3.getSamples(SAMPLE_NUM) < 0.005)) {
-            //moveForwardWallEncoder(); 
-        }else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005){ // left wall gone
+            //moveForwardWallEncoder();
+        } else if(IRP_2.getSamples(SAMPLE_NUM) < 0.005) { // left wall gone
             //moveForwardRightWall();
-        }else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005){ // right wall gone
+        } else if(IRP_3.getSamples(SAMPLE_NUM) < 0.005) { // right wall gone
             //moveForwardLeftWall();
-        }else{
-        // will move forward using encoders only 
-        // if cell ahead doesn't have a wall on either one side or both sides
+        } else {
+            // will move forward using encoders only
+            // if cell ahead doesn't have a wall on either one side or both sides
 
             int pulseCount = (encoder0.getPulses()-count) % 5600;
             if (pulseCount > 5400 && pulseCount < 5800) {
@@ -257,12 +250,12 @@
         right_motor.brake();
     }
 }
- 
+
 void printDipFlag()
 {
     if (DEBUGGING) serial.printf("Flag value is %d", dipFlags);
 }
- 
+
 void enableButton1()
 {
     dipFlags |= BUTTON1_FLAG;
@@ -303,7 +296,7 @@
     dipFlags &= ~BUTTON4_FLAG;
     printDipFlag();
 }
- 
+
 void pidOnEncoders()
 {
     int count0;
@@ -314,10 +307,9 @@
     double kp = 0.00013;
     double kd = 0.00016;
     int prev = 0;
- 
+
     int counter = 0;
-    while(1)
-    {
+    while(1) {
         count0 = encoder0.getPulses();
         count1 = encoder1.getPulses();
         int x = count0 - count1;
@@ -325,22 +317,19 @@
         double kppart = kp * x;
         double kdpart = kd * (x-prev);
         double d = kppart + kdpart;
-        
+
         //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
-        if( x < diff - 60 ) // count1 is bigger, right wheel pushed forward
-        {
+        if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
             left_motor.move( -d );
             right_motor.move( d );
-        }
-        else if( x > diff + 60 )
-        {
+        } else if( x > diff + 60 ) {
             left_motor.move( -d );
             right_motor.move( d );
         }
         // else
         // {
         //     left_motor.brake();
-        //     right_motor.brake();   
+        //     right_motor.brake();
         // }
         prev = x;
         counter++;
@@ -348,8 +337,9 @@
             break;
     }
 }
- 
-void nCellEncoderAndIR(double cellCount){
+
+void nCellEncoderAndIR(double cellCount)
+{
     double currentError = 0;
     double previousError = 0;
     double derivError = 0;
@@ -375,29 +365,17 @@
 
     int state = 0;
 
-    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){
+    while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
         receiverTwoReading = IRP_2.getSamples(100);
         receiverThreeReading = IRP_3.getSamples(100);
         receiverOneReading = IRP_1.getSamples(100);
         receiverFourReading = IRP_4.getSamples(100);
 
-        if(  receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){
-            if (currDir % 4 == 0){
-                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
-            }
-            else if (currDir % 4 == 1){
-                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
-            }
-            else if (currDir % 4 == 2){
-                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
-            }
-            else if (currDir % 4 == 3){
-                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
-            }
+        if(  receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ) {
             break;
         }
 
-       if((receiverThreeReading <  3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))){
+        if((receiverThreeReading <  3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 3*IRP_2.sensorAvg/(averageDivL))) {
             // both sides gone
             int prev0 = encoder0.getPulses();
             int prev1 = encoder1.getPulses();
@@ -405,60 +383,18 @@
             int diff1 = desiredCount1 - prev1;
             int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
             moveForwardEncoder(valToPass);
-        }
-        else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR){// right wall gone
-            // if (currDir % 4 == 0){
-            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
-            // }
-            // else if (currDir % 4 == 1){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
-            // }
-            // else if (currDir % 4 == 2){
-            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
-            // }
-            // else if (currDir % 4 == 3){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
-            // }
-            // RED RED RED RED RED
+        } else if (receiverThreeReading < 3*IRP_3.sensorAvg/averageDivR) { // right wall gone
             state = 1;
             redLed.write(0);
             greenLed.write(1);
             blueLed.write(1);
-        }else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL){// left wall gone
-            // if (currDir % 4 == 0){
-            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
-            // }
-            // else if (currDir % 4 == 1){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
-            // }
-            // else if (currDir % 4 == 2){
-            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
-            // }
-            // else if (currDir % 4 == 3){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
-            // }
+        } else if (receiverTwoReading < 3*IRP_2.sensorAvg/averageDivL) { // left wall gone
             // BLUE BLUE BLUE BLUE
             state = 2;
             redLed.write(1);
             greenLed.write(1);
             blueLed.write(0);
-        }else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){
-            // if (currDir % 4 == 0){
-            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
-            //     wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
-            // }
-            // else if (currDir % 4 == 1){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
-            // }
-            // else if (currDir % 4 == 2){
-            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
-            //     wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
-            // }
-            // else if (currDir % 4 == 3){
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
-            //     wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
-            // }
+        } else if ((receiverTwoReading > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))) {
             // both walls there
             state = 0;
             redLed.write(1);
@@ -466,20 +402,20 @@
             blueLed.write(1);
         }
 
-        switch(state){
-            case(0):{ // both walls there
+        switch(state) {
+            case(0): { // both walls there
                 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
                 break;
             }
-            case(1):{// RED RED RED RED RED
+            case(1): { // RED RED RED RED RED
                 currentError = (receiverTwoReading - IRP_2.sensorAvg/initAverageL) - (IRP_2.sensorAvg/initAverageL);
-                break;   
+                break;
             }
-            case(2):{// blue
+            case(2): { // blue
                 currentError =  (IRP_3.sensorAvg/initAverageR) - (receiverThreeReading - IRP_3.sensorAvg/initAverageR);
                 break;
             }
-            default:{
+            default: {
                 currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL)  - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR);
                 break;
             }
@@ -494,58 +430,98 @@
         } else { // r is faster than L. speed up l, slow down r
             rightSpeed = HIGH_PWM_VOLTAGE_R + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
             leftSpeed = HIGH_PWM_VOLTAGE_L - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
-        }   
+        }
         if (leftSpeed > 0.30) leftSpeed = 0.30;
         if (leftSpeed < 0) leftSpeed = 0;
         if (rightSpeed > 0.30) rightSpeed = 0.30;
         if (rightSpeed < 0) rightSpeed = 0;
-    
+
         right_motor.forward(rightSpeed);
         left_motor.forward(leftSpeed);
         pidOnEncoders();
-    
+
         previousError = currentError;
     }
-    if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1){
-        if (currDir % 4 == 0){
-        mouseY += 1;
-        }
-        else if (currDir % 4 == 1){
+    if (encoder0.getPulses() >= 0.6*desiredCount0 && encoder1.getPulses() >= 0.6*desiredCount1) {
+        if (currDir % 4 == 0) {
+            mouseY += 1;
+        } else if (currDir % 4 == 1) {
             mouseX + 1;
-        }
-        else if (currDir % 4 == 2){
+        } else if (currDir % 4 == 2) {
             mouseY -= 1;
-        }
-        else if (currDir % 4 == 3){
+        } else if (currDir % 4 == 3) {
             mouseX -= 1;
         }
+
+        // the mouse has moved forward, we need to update the wall map now
+        receiverOneReading = IRP_1.getSamples(50);
+        receiverTwoReading = IRP_2.getSamples(50);
+        receiverThreeReading = IRP_3.getSamples(50);
+        receiverFourReading = IRP_4.getSamples(50);
+
+        if (receiverOneReading >= 0.5f && receiverFourReading >= 0.5f) {
+            if (currDir % 4 == 0) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
+            } else if (currDir % 4 == 1) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
+            } else if (currDir % 4 == 2) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
+            } else if (currDir % 4 == 3) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
+            }
+        }
+        if (receiverThreeReading >= 0.1f) {
+            if (currDir % 4 == 0) {
+                wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL;
+            } else if (currDir % 4 == 1) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= B_WALL;
+            } else if (currDir % 4 == 2) {
+                wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL;
+            } else if (currDir % 4 == 3) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= F_WALL;
+            }
+        }
+        if (receiverTwoReading >= 0.1f) {
+            if (currDir % 4 == 0) {
+                wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL;
+            } else if (currDir % 4 == 1) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= F_WALL;
+            } else if (currDir % 4 == 2) {
+                wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= R_WALL;
+            } else if (currDir % 4 == 3) {
+                wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL;
+            }
+        }
     }
-    
- 
+
     left_motor.brake();
     right_motor.brake();
 }
 
-bool isWallInFront(int x, int y){
+bool isWallInFront(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & F_WALL);
 }
-bool isWallInBack(int x, int y){
+bool isWallInBack(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & B_WALL);
 }
-bool isWallOnRight(int x, int y){
+bool isWallOnRight(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & R_WALL);
 }
-bool isWallOnLeft(int x, int y){
+bool isWallOnLeft(int x, int y)
+{
     return (wallArray[MAZE_LEN - y - 1][x]  & L_WALL);
 }
 
-int chooseNextMovement(){
+int chooseNextMovement()
+{
     int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
-    if (goingToCenter && (currentDistance == 0)){
+    if (goingToCenter && (currentDistance == 0)) {
         goingToCenter = false;
         changeManhattanDistance(goingToCenter);
-    }
-    else if (!goingToCenter && (currentDistance == 0)){
+    } else if (!goingToCenter && (currentDistance == 0)) {
         goingToCenter == true;
         changeManhattanDistance(goingToCenter);
     }
@@ -555,7 +531,7 @@
     int currY = 0;
     int currDist = 0;
     int dirToGo = 0;
-    if (!justTurned){
+    if (!justTurned) {
         cellsToVisit.push(make_pair(mouseX, mouseY));
         while (!cellsToVisit.empty()) {
             pair<int, int> curr = cellsToVisit.top();
@@ -565,7 +541,7 @@
             currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
             int minDist = INT_MAX;
 
-             if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
+            if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
                 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
                     if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
                         minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
@@ -747,8 +723,7 @@
                 }
             }
         }
-    }
-    else{
+    } else {
         justTurned = false;
         dirToGo = 0;
     }
@@ -756,12 +731,14 @@
     return dirToGo;
 }
 
-bool hasVisited(int x, int y){
+bool hasVisited(int x, int y)
+{
     return visitedCells[MAZE_LEN - 1 - y][x];
 }
 
-void changeManhattanDistance(bool headCenter){
-    if (headCenter){
+void changeManhattanDistance(bool headCenter)
+{
+    if (headCenter) {
         for (int i = 0; i < MAZE_LEN; i++) {
             for (int j = 0; j < MAZE_LEN; j++) {
                 distanceToCenter[i][j] = manhattanDistances[i][j];
@@ -772,9 +749,8 @@
                 manhattanDistances[i][j] = distanceToStart[i][j];
             }
         }
-    }
-    else {
-         for (int i = 0; i < MAZE_LEN; i++) {
+    } else {
+        for (int i = 0; i < MAZE_LEN; i++) {
             for (int j = 0; j < MAZE_LEN; j++) {
                 distanceToStart[i][j] = manhattanDistances[i][j];
             }
@@ -787,29 +763,30 @@
     }
 }
 
-void initializeDistances(){
+void initializeDistances()
+{
     for (int i = 0; i < MAZE_LEN / 2; i++) {
-            for (int j = 0; j < MAZE_LEN / 2; j++) {
-                distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
-            }
+        for (int j = 0; j < MAZE_LEN / 2; j++) {
+            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
         }
-        for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
-            for (int j = 0; j < MAZE_LEN / 2; j++) {
-                distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
-            }
+    }
+    for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+        for (int j = 0; j < MAZE_LEN / 2; j++) {
+            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
         }
-        for (int i = 0; i < MAZE_LEN / 2; i++) {
-            for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
-                distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
-            }
+    }
+    for (int i = 0; i < MAZE_LEN / 2; i++) {
+        for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
         }
-        for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
-            for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
-                distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
-            }
+    }
+    for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
+        for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
+            distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
         }
+    }
 }
-  
+
 int main()
 {
     //Set highest bandwidth.
@@ -817,30 +794,30 @@
     initializeDistances();
     serial.baud(9600);
     //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
- 
+
     wait (0.1);
- 
+
     redLed.write(1);
     greenLed.write(0);
     blueLed.write(1);
- 
+
     //left_motor.forward(0.1);
     //right_motor.forward(0.1);
- 
+
     // PA_1 is A of right
     // PA_0 is B of right
     // PA_5 is A of left
     // PB_3 is B of left
     //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
     //    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
- 
+
     // TODO: Setting all the registers and what not for Quadrature Encoders
     /*    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
         RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
         GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
         GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
         */
- 
+
     // set GPIO pins to alternate for the pins corresponding to A/B for eacah encoder, and 2 alternate function registers need to be selected for each type
     // of alternate function specified
     // 4 modes sets AHB1ENR
@@ -854,21 +831,19 @@
     // SMCR - encoder mode
     // CR1 reenabales
     // then read CNT reg of timer
- 
- 
+
+
     dipButton1.rise(&enableButton1);
     dipButton2.rise(&enableButton2);
     dipButton3.rise(&enableButton3);
     dipButton4.rise(&enableButton4);
- 
+
     dipButton1.fall(&disableButton1);
     dipButton2.fall(&disableButton2);
     dipButton3.fall(&disableButton3);
     dipButton4.fall(&disableButton4);
 
     // if(dipFlags == 0x1){
-        turnRight180();
-        wait_ms(1000);
     // }else{
     //     turnRight();
     //     IRP_1.calibrateSensor();
@@ -898,7 +873,7 @@
     //wait_ms(1500);
     int nextMovement = 0;
     while (1) {
-       // prevEncoder0Count = encoder0.getPulses();
+        // prevEncoder0Count = encoder0.getPulses();
 //        prevEncoder1Count = encoder1.getPulses();
 //
 //        if (!overrideFloodFill){
@@ -935,7 +910,7 @@
 //        }
 //        currEncoder0Count = encoder0.getPulses();
 //        currEncoder1Count = encoder1.getPulses();
-//       
+//
 //        if (!justTurned && (currEncoder0Count <= prevEncoder0Count + 100) && (currEncoder1Count <= prevEncoder1Count + 100) && !overrideFloodFill){
 //            overrideFloodFill = true;
 //        }
@@ -969,7 +944,7 @@
         // nCellEncoderAndIR(5);
         // break;
         // turnRight180();
- 
+
         // int number = rand() % 4 + 1;
         // switch(number){
         //     case(1):{//turn right
@@ -981,7 +956,7 @@
         //         break;
         //     }
         //     case(3):{// keep going
- 
+
         //         break;
         //     }
         //     case(4):{// turnaround
@@ -992,26 +967,26 @@
         //         break;
         //     }
         // }
- 
+
         // float irbase2 = IRP_2.sensorAvg/initAverageL/averageDivL;
         // float irbase3 = IRP_3.sensorAvg/initAverageR/averageDivR;
- 
+
         // float ir3  = IRP_2.getSamples(100)/initAverageL;
         // float ir2  = IRP_3.getSamples(100)/initAverageR;
-        
- 
+
+
 
         /*
         counter2++;
-        counter3++; 
+        counter3++;
         ir2tot += IRP_2.getSamples(100);
         ir3tot += IRP_3.getSamples(100);
- 
- 
+
+
         ir2 = ir2tot/counter2;
         ir3 = ir3tot/counter3;
-    
- 
+
+
         serial.printf("IRS= >: %f, %f \r\n", ir2, ir3);
         */
         //serial.printf("%f, %f \n", IRP_2.sensorAvg/initAverageL/averageDivL, IRP_3.sensorAvg/initAverageR/averageDivR);
@@ -1020,17 +995,17 @@
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100));
         //serial.printf("IRSAvg= >: %f, %f \r\n", ir2, ir3);
         //serial.printf("IRSAvg= >: %f, %f \r\n", IRP_2.sensorAvg, IRP_3.sensorAvg);
- 
- 
+
+
         ////////////////////////////////////////////////////////////////
- 
+
         //nCellEncoderAndIR(3);
         //break;
- 
+
         //serial.printf("IRS= >: %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100));
 
         //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ));
-  
+
         //break;
         // moveForwardCellEncoder(1);
         // wait(0.5);
@@ -1041,11 +1016,11 @@
         // handleTurns();
         //break;
         //pidOnEncoders();
-       // moveForwardUntilWallIr();
+        // moveForwardUntilWallIr();
         //serial.printf("Pulse Count=> e0:%d, e1:%d      \r\n", encoder0.getPulses(),encoder1.getPulses());
-         double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
+        double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
         serial.printf("IRS= >: %f, %f, %f, %f, %f \r\n", IRP_1.getSamples( 100 ), IRP_2.getSamples( 100 ), IRP_3.getSamples( 100 ), IRP_4.getSamples(100), currentError );
- 
+
         //reading = Rec_4.read();
 //        serial.printf("reading: %f\n", reading);
     }
--- a/main.h	Wed May 24 02:40:36 2017 +0000
+++ b/main.h	Wed May 24 21:47:21 2017 +0000
@@ -111,7 +111,7 @@
 
 stack< pair<int, int> > cellsToVisit;
 
-int currDir = 102;              // modulo this to keep track of the current direction of the mouse!
+int currDir = 100;              // modulo this to keep track of the current direction of the mouse!
 // 0 = forward, 1 = right, 2 = down, 3 = left
 int wallArray[16][16] = {0};    // array to keep track of the walls
 int visitedCells[16][16] = {0};     // array to keep track of the mouse's current location