Mouse code for the MacroRat
Diff: main.cpp
- Revision:
- 27:02ce1040f331
- Parent:
- 26:d20f1adac2d3
- Child:
- 28:8126a4d620e8
diff -r d20f1adac2d3 -r 02ce1040f331 main.cpp --- a/main.cpp Sun May 21 09:58:54 2017 +0000 +++ b/main.cpp Sun May 21 10:30:19 2017 +0000 @@ -46,9 +46,9 @@ float ir4base = 0.0; float initAverageL = 8.28; -float averageDivL = 30.5; //blue +float averageDivL = 29.5; //blue float initAverageR = 8.75; //4.5 -float averageDivR = 30.5; //red +float averageDivR = 29.5; //red float averageDivUpper = 0.9; //IRSAvg= >: 0.143701, 0.128285 @@ -363,8 +363,6 @@ double kd = 0.00019; int prev = 0; - - double speed0 = 0.11; double speed1 = 0.13; right_motor.move(speed0); @@ -607,30 +605,28 @@ int state = 0; while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1){ - receiverTwoReading = IRP_2.getSamples(100); - receiverThreeReading = IRP_3.getSamples(100); - // previr2 = receiverTwoReading; - // previr3 = receiverThreeReading; - receiverOneReading = IRP_1.getSamples(100); - receiverFourReading = IRP_4.getSamples(100); + receiverTwoReading = IRP_2.getSamples(60); + receiverThreeReading = IRP_3.getSamples(60); + receiverOneReading = IRP_1.getSamples(60); + receiverFourReading = IRP_4.getSamples(60); - if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){ + if( receiverOneReading > IRP_1.sensorAvg*0.70 || receiverFourReading > IRP_4.sensorAvg*0.70 ){ if (currDir % 4 == 0){ - wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= F_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 1){ - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= R_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 2){ - wallArray[MAZE_LEN - 1 - (mouseY - 1)][mouseX] |= L_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; } else if (currDir % 4 == 3){ - wallArray[MAZE_LEN - 1 - (mouseY)][mouseX-1] |= B_WALL; + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL; } break; } - if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL)) ){ + if((receiverThreeReading < 1.3*IRP_3.sensorAvg/(averageDivR)) && (receiverTwoReading < 1.3*IRP_2.sensorAvg/(averageDivL))){ // both sides gone redLed.write(1); greenLed.write(1); @@ -660,42 +656,42 @@ moveForwardEncoder(valToPass); } else if (receiverThreeReading < 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; - } + // 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 state = 1; redLed.write(0); greenLed.write(1); blueLed.write(1); }else if (receiverTwoReading < 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; - } + // 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; + // } // 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))){ + }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; @@ -781,16 +777,16 @@ } bool isWallInFront(int x, int y){ - return (wallArray[MAZE_LEN - y - 1][x] & 0x1); + return (wallArray[MAZE_LEN - y - 1][x] & F_WALL); } bool isWallInBack(int x, int y){ - return (wallArray[MAZE_LEN - y - 1][x] & 0x8); + return (wallArray[MAZE_LEN - y - 1][x] & B_WALL); } bool isWallOnRight(int x, int y){ - return (wallArray[MAZE_LEN - y - 1][x] & 0x4); + return (wallArray[MAZE_LEN - y - 1][x] & R_WALL); } bool isWallOnLeft(int x, int y){ - return (wallArray[MAZE_LEN - y - 1][x] & 0x2); + return (wallArray[MAZE_LEN - y - 1][x] & L_WALL); } int chooseNextMovement(){ @@ -825,17 +821,17 @@ minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1]; } } - if (currX - 1 >= 0 && !isWallOnLeft((unsigned) currX, (unsigned) currY)) { + if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) { if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) { minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1]; } } - if (currY + 1 < MAZE_LEN && !isWallInFront((unsigned) currX, (unsigned) currY)) { + if (currY + 1 < MAZE_LEN && !isWallInFront( currX, currY)) { if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) { minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX]; } } - if (currY - 1 >= 0 && !isWallInBack((unsigned) currX, (unsigned) currY)) { + if (currY - 1 >= 0 && !isWallInBack(currX, currY)) { if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) { minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX]; } @@ -868,8 +864,6 @@ if (currDist > minDist) continue; if (currDist <= minDist) { -// cout << "Changing the following coordinate ( " << currX << "," << currY << ") from " -// << manhattanDistances[MAZE_LEN - 1 - currY][currX] << " to " << minDist + 1 << endl; manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1; } if (hasVisited(currX, currY)) {