Mouse code for the MacroRat
Revision 26:d20f1adac2d3, committed 2017-05-21
- Comitter:
- sahilmgandhi
- Date:
- Sun May 21 09:58:54 2017 +0000
- Parent:
- 25:f827a8b7880e
- Child:
- 27:02ce1040f331
- Commit message:
- Added floodfill to the code ... need to tune it now to get the wall array properly fixed!;
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 21 08:52:43 2017 +0000 +++ b/main.cpp Sun May 21 09:58:54 2017 +0000 @@ -1,20 +1,23 @@ #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 -#include <stdlib.h> #include "ITG3200.h" #include "stm32f4xx.h" #include "QEI.h" - + /* Constants for when HIGH_PWM_VOLTAGE = 0.2 #define IP_CONSTANT 6 #define II_CONSTANT 0 #define ID_CONSTANT 1 */ - + // Constants for when HIGH_PWM_VOLTAGE = 0.1 // #define IP_CONSTANT 8.85 // #define II_CONSTANT 0.005 @@ -22,104 +25,85 @@ #define IP_CONSTANT 8.5 #define II_CONSTANT 0.095 #define ID_CONSTANT 6.85 - -const int desiredCount180 = 2850; + +const int desiredCount180 = 2800; const int desiredCountR = 1575; const int desiredCountL = 1650; - + const int oneCellCount = 5400; -const int oneCellCountMomentum = 4700; // one cell count is actually approximately 5400, but this value is considering momentum! - +const int oneCellCountMomentum = 4700;//4800; // one cell count is actually approximately 5400, but this value is considering momentum! + float receiverOneReading = 0.0; float receiverTwoReading = 0.0; float receiverThreeReading = 0.0; float receiverFourReading = 0.0; +float ir1base = 0.0; +float ir2base = 0.0; + +float ir3base = 0.0; + +float ir4base = 0.0; float initAverageL = 8.28; -float averageDivL = 29.5; //blue +float averageDivL = 30.5; //blue float initAverageR = 8.75; //4.5 -float averageDivR = 29.5; //red +float averageDivR = 30.5; //red float averageDivUpper = 0.9; +//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 + +float noWallR = 0.007; +float noWallL = 0.007; + void pidOnEncoders(); - - + + void turnLeft() { double speed0 = 0.11; double speed1 = -0.13; - + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - + int desiredCount0 = initial0 - desiredCountL; int desiredCount1 = initial1 + desiredCountL; - + int count0 = initial0; int count1 = initial1; - + double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - - + while(1) { - + if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); - + error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - turnFlag = 0; // zeroing out the flags! - currDir -= 1; -} - -void turnRight() -{ - double speed0 = -0.11; - double speed1 = 0.13; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 + desiredCountR; - int desiredCount1 = initial1 - desiredCountR; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - + right_motor.move(speed0); left_motor.move(speed1); counter = 0; @@ -128,93 +112,45 @@ right_motor.brake(); left_motor.brake(); } - + if (counter > 60) { break; } } - + right_motor.brake(); left_motor.brake(); - turnFlag = 0; - currDir += 1; + turnFlag = 0; // zeroing out the flags! + currDir -= 1; } - -void turnLeft180() + +void turnRight() { - double speed0 = 0.15; - double speed1 = -0.15; - + double speed0 = -0.11; + double speed1 = 0.13; + int counter = 0; int initial0 = encoder0.getPulses(); int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 - desiredCountL*2; - int desiredCount1 = initial1 + desiredCountL*2; - + + int desiredCount0 = initial0 + desiredCountR; + int desiredCount1 = initial1 - desiredCountR; + int count0 = initial0; int count1 = initial1; - + double error0 = count0 - desiredCount0; double error1 = count1 - desiredCount1; - - + while(1) { - + if(!(abs(error0) < 1) && !(abs(error1) < 1)) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); - + error0 = count0 - desiredCount0; error1 = count1 - desiredCount1; - - right_motor.move(speed0); - left_motor.move(speed1); - counter = 0; - } else { - counter++; - right_motor.brake(); - left_motor.brake(); - } - - if (counter > 60) { - break; - } - } - - right_motor.brake(); - left_motor.brake(); - currDir -= 2; -} - -void turnRight180() -{ - double speed0 = -0.16; - double speed1 = 0.16; - - int counter = 0; - int initial0 = encoder0.getPulses(); - int initial1 = encoder1.getPulses(); - - int desiredCount0 = initial0 + desiredCount180; - int desiredCount1 = initial1 - desiredCount180; - - int count0 = initial0; - int count1 = initial1; - - double error0 = count0 - desiredCount0; - double error1 = count1 - desiredCount1; - - - while(1) { - - if(!(abs(error0) < 1) && !(abs(error1) < 1)) { - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - - error0 = count0 - desiredCount0; - error1 = count1 - desiredCount1; - + right_motor.move(speed0); left_motor.move(speed1); counter = 0; @@ -223,7 +159,102 @@ right_motor.brake(); left_motor.brake(); } - + + if (counter > 60) { + break; + } + } + + right_motor.brake(); + left_motor.brake(); + turnFlag = 0; + currDir += 1; +} + +void turnLeft180() +{ + double speed0 = 0.15; + double speed1 = -0.15; + + int counter = 0; + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); + + int desiredCount0 = initial0 - desiredCountL*2; + int desiredCount1 = initial1 + desiredCountL*2; + + int count0 = initial0; + int count1 = initial1; + + double error0 = count0 - desiredCount0; + double error1 = count1 - desiredCount1; + + + while(1) { + + if(!(abs(error0) < 1) && !(abs(error1) < 1)) { + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = count0 - desiredCount0; + error1 = count1 - desiredCount1; + + right_motor.move(speed0); + left_motor.move(speed1); + counter = 0; + } else { + counter++; + right_motor.brake(); + left_motor.brake(); + } + + if (counter > 60) { + break; + } + } + + right_motor.brake(); + left_motor.brake(); + currDir -= 2; +} + +void turnRight180() +{ + double speed0 = -0.16; + double speed1 = 0.16; + + int counter = 0; + int initial0 = encoder0.getPulses(); + int initial1 = encoder1.getPulses(); + + int desiredCount0 = initial0 + desiredCount180; + int desiredCount1 = initial1 - desiredCount180; + + int count0 = initial0; + int count1 = initial1; + + double error0 = count0 - desiredCount0; + double error1 = count1 - desiredCount1; + + + while(1) { + + if(!(abs(error0) < 1) && !(abs(error1) < 1)) { + count0 = encoder0.getPulses(); + count1 = encoder1.getPulses(); + + error0 = count0 - desiredCount0; + error1 = count1 - desiredCount1; + + right_motor.move(speed0); + left_motor.move(speed1); + counter = 0; + } else { + counter++; + right_motor.brake(); + left_motor.brake(); + } + if (counter > 60) { break; } @@ -232,11 +263,11 @@ left_motor.brake(); currDir += 2; } - + 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); @@ -244,88 +275,26 @@ 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 < IRP_3.sensorAvg/averageDivR){ + if (receiverThreeReading < ir3base){ // redLed.write(1); // blueLed.write(0); turnFlag |= RIGHT_FLAG; } - else if (receiverTwoReading < IRP_2.sensorAvg/averageDivL){ + else if (receiverTwoReading < ir2base){ // redLed.write(0); // blueLed.write(1); turnFlag |= LEFT_FLAG; } pidOnEncoders(); } - + left_motor.brake(); right_motor.brake(); } - -void handleTurns(){ - if (turnFlag == 0x1){ - // moveForwardCellEncoder(0.3); - turnLeft(); - } - else if (turnFlag == 0x2){ - // moveForwardCellEncoder(0.3); - turnRight(); - } - else if (turnFlag == 0x3){ - // moveForwardCellEncoder(0.3); - turnLeft(); - turnRight(); - } -} - -void pidBrake(){ - - int count0; - int count1; - count0 = encoder0.getPulses(); - count1 = encoder1.getPulses(); - int initial0 = count0; - int initial1 = count1; - double kp = 0.00011; - - - - int error = count0 - count1; - - int counter = 0; - right_motor.move(0.7); - left_motor.move(0.7); - - double speed0 = 0.7; - double speed1 = 0.7; - - while(1) - { - if(abs(error) < 3){ - right_motor.brake(); - left_motor.brake(); - counter++; - }else{ - count0 = encoder0.getPulses() - initial0; - count1 = encoder1.getPulses() - initial1; - error = count0 - count1; - speed0 = -error*kp + speed0; - speed1 = error*kp + speed1; - - right_motor.move(speed0); - left_motor.move(speed1); - - counter = 0; - } - if (counter > 10){ - break; - } - - } - return; -} - -void moveForwardEncoder(){ - + + +void moveForwardEncoder(double num){ + int count0; int count1; count0 = encoder0.getPulses(); @@ -337,15 +306,13 @@ double kd = 0.00019; int prev = 0; - - double speed0 = 0.10; double speed1 = 0.12; right_motor.move(speed0); left_motor.move(speed1); - - - while((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-200) && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-200)) { + + + 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 )); @@ -375,18 +342,17 @@ // } prev = x; } - + //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } - - + + void moveForwardWallEncoder(){ - - int count0; + int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); @@ -451,7 +417,7 @@ left_motor.brake(); return; } - + void moveForwardUntilWallIr() { double currentError = 0; @@ -521,12 +487,12 @@ right_motor.brake(); } } - + void printDipFlag() { if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } - + void enableButton1() { dipFlags |= BUTTON1_FLAG; @@ -567,7 +533,7 @@ dipFlags &= ~BUTTON4_FLAG; printDipFlag(); } - + void pidOnEncoders() { int count0; @@ -578,7 +544,7 @@ double kp = 0.00011; double kd = 0.00014; int prev = 0; - + int counter = 0; while(1) { @@ -612,7 +578,7 @@ break; } } - + void nCellEncoderAndIR(double cellCount){ double currentError = 0; double previousError = 0; @@ -640,54 +606,31 @@ int state = 0; - - while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1 && receiverOneReading < IRP_1.sensorAvg*0.8 && receiverFourReading < IRP_4.sensorAvg*0.8){ + 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); - - //if ((receiverOneReading+receiverFourReading)/2 > ((IRP_1.sensorAvg+IRP_4.sensorAvg)/2)*0.15 ){ - if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){ - // almost to the end - right_motor.move(-0.15); - left_motor.move(-0.15); - - wait_ms(150); - right_motor.brake(); - left_motor.brake(); - - redLed.write(1); - greenLed.write(1); - blueLed.write(1); - wait_ms(200); - redLed.write(1); - greenLed.write(1); - blueLed.write(0); - wait_ms(200); - - + if( receiverOneReading > IRP_1.sensorAvg*0.7 || receiverFourReading > IRP_4.sensorAvg*0.7 ){ + if (currDir % 4 == 0){ + wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= F_WALL; + } + else if (currDir % 4 == 1){ + wallArray[MAZE_LEN - 1 - (mouseY)][mouseX+1] |= R_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] |= B_WALL; + } + break; + } - redLed.write(1); - greenLed.write(0); - blueLed.write(1); - wait_ms(200); - redLed.write(0); - greenLed.write(1); - blueLed.write(1); - - - - - //moveForwardWallEncoder(); - - - return; - - }else 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); @@ -708,20 +651,67 @@ redLed.write(1); greenLed.write(1); blueLed.write(0); - moveForwardEncoder(); - }else if (receiverThreeReading < IRP_3.sensorAvg/averageDivR){// right wall gone + + int prev0 = encoder0.getPulses(); + int prev1 = encoder1.getPulses(); + int diff0 = desiredCount0 - prev0; + int diff1 = desiredCount1 - prev1; + int valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); + 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; + } // 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; + } // 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; + } // both walls there state = 0; redLed.write(1); @@ -748,12 +738,7 @@ break; } } - //currentError = ( receiverTwoReading - IRP_2.sensorAvg/initAverageL) - ( receiverThreeReading - IRP_3.sensorAvg/initAverageR); - - - - sumError += currentError; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; @@ -768,55 +753,346 @@ 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; ir2 = IRP_2.getSamples( SAMPLE_NUM ); ir3 = IRP_3.getSamples( SAMPLE_NUM ); - + } + if (currDir % 4 == 0){ + mouseY += 1; + } + else if (currDir % 4 == 1){ + mouseX + 1; } - - - + else if (currDir % 4 == 2){ + mouseY -= 1; + } + else if (currDir % 4 == 3){ + mouseX -= 1; + } + left_motor.brake(); right_motor.brake(); return; } +bool isWallInFront(int x, int y){ + return (wallArray[MAZE_LEN - y - 1][x] & 0x1); +} +bool isWallInBack(int x, int y){ + return (wallArray[MAZE_LEN - y - 1][x] & 0x8); +} +bool isWallOnRight(int x, int y){ + return (wallArray[MAZE_LEN - y - 1][x] & 0x4); +} +bool isWallOnLeft(int x, int y){ + return (wallArray[MAZE_LEN - y - 1][x] & 0x2); +} + +int chooseNextMovement(){ + int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX]; + if (goingToCenter && (currentDistance == 0)){ + goingToCenter = false; + changeManhattanDistance(goingToCenter); + } + else if (!goingToCenter && (currentDistance == 0)){ + goingToCenter == true; + changeManhattanDistance(goingToCenter); + } + + visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1; + int currX = 0; + int currY = 0; + int currDist = 0; + int dirToGo = 0; + if (!justTurned){ + cellsToVisit.push(make_pair(mouseX, mouseY)); + while (!cellsToVisit.empty()) { + pair<int, int> curr = cellsToVisit.top(); + cellsToVisit.pop(); + currX = curr.first; + currY = curr.second; + 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 (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) { + if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1]; + } + } + if (currX - 1 >= 0 && !isWallOnLeft((unsigned) currX, (unsigned) 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 (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 (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX]; + } + } + } else { + if (currX + 1 < MAZE_LEN) { + if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1]; + } + } + if (currX - 1 >= 0) { + if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1]; + } + } + if (currY + 1 < MAZE_LEN) { + if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX]; + } + } + if (currY - 1 >= 0) { + if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) { + minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX]; + } + } + } + + if (minDist == INT_MAX) + continue; + 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)) { + if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) { + cellsToVisit.push(make_pair(currX, currY + 1)); + } + if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) { + cellsToVisit.push(make_pair(currX + 1, currY)); + } + if (currY - 1 >= 0 && !isWallInBack(currX, currY)) { + cellsToVisit.push(make_pair(currX, currY - 1)); + } + if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) { + cellsToVisit.push(make_pair(currX - 1, currY)); + } + } else { + if (currY + 1 < MAZE_LEN) { + cellsToVisit.push(make_pair(currX, currY + 1)); + } + if (currX + 1 < MAZE_LEN) { + cellsToVisit.push(make_pair(currX + 1, currY)); + } + if (currY - 1 >= 0) { + cellsToVisit.push(make_pair(currX, currY - 1)); + } + if (currX - 1 >= 0) { + cellsToVisit.push(make_pair(currX - 1, currY)); + } + } + } + + int minDistance = INT_MAX; + if (currDir % 4 == 0) { + 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 = 1; + } + } + 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 = 2; + } + } + 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 = 3; + } + } + 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 = 4; + } + } + } else if (currDir % 4 == 2) { + if (mouseX - 1 >= 0 && !isWallOnRight(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 (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 (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 (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 (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 (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) { + minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX]; + dirToGo = 2; + } + } + if (mouseX + 1 < MAZE_LEN && !isWallInFront(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 (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 (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 (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 (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 (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) { + minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1]; + dirToGo = 4; + } + } + } + } + else{ + justTurned = false; + dirToGo = 0; + } + + return dirToGo; +} + +bool hasVisited(int x, int y){ + return visitedCells[MAZE_LEN - 1 - y][x]; +} + +void changeManhattanDistance(bool headCenter){ + if (headCenter){ + for (int i = 0; i < MAZE_LEN / 2; i++) { + for (int j = 0; j < MAZE_LEN / 2; j++) { + manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(7 - i); + } + } + for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { + for (int j = 0; j < MAZE_LEN / 2; j++) { + manhattanDistances[MAZE_LEN - 1 - j][i] = abs(7 - j) + abs(8 - i); + } + } + for (int i = 0; i < MAZE_LEN / 2; i++) { + for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { + manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(7 - i); + } + } + for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) { + for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) { + manhattanDistances[MAZE_LEN - 1 - j][i] = abs(8 - j) + abs(8 - i); + } + } + } + else { + for (int i = 0; i < MAZE_LEN / 2; i++) { + for (int j = 0; j < MAZE_LEN / 2; j++) { + manhattanDistances[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++) { + manhattanDistances[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++) { + manhattanDistances[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++) { + manhattanDistances[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i); + } + } + } +} + int main() { //Set highest bandwidth. //gyro.setLpBandwidth(LPFBW_42HZ); 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 ); - + // 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 @@ -830,100 +1106,71 @@ // 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(); + // IRP_4.calibrateSensor(); + // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= L_WALL; + // wallArray[MAZE_LEN - 1 - (mouseY + 1)][mouseX] |= R_WALL; + + // wait_ms(300); + // turnLeft(); + // wait_ms(300); + // } + + + // init the wall, and mouse loc arrays: + wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE; + visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1; + //right_motor.forward( 0.2 ); //left_motor.forward( 0.2 ); - turnRight180(); - wait_ms(1500); + //turnRight180(); + //wait_ms(1500); + int nextMovement = 0; + while (1) { + nextMovement = chooseNextMovement(); + if (nextMovement == 0){ + nCellEncoderAndIR(1); + } + else if (nextMovement == 1){ + justTurned = true; + turnRight(); + } + else if (nextMovement == 2){ + justTurned = true; + turnLeft(); + } + else if (nextMovement == 3){ + nCellEncoderAndIR(1); + } + else if (nextMovement == 4){ + justTurned = true; + turnRight180(); + } + wait_ms(1000); // reduce this once we fine tune this! - - while (1) { //wait_ms(1500); //turnRight(); //wait_ms(1500); //turnLeft(); - - - // float ir2 = IRP_2.getSamples(100); - // float ir3 = IRP_3.getSamples(100); - // float ir1 = IRP_1.getSamples(100); - // float ir4 = IRP_4.getSamples(100); - - - // if( ir1 > IRP_1.sensorAvg*0.3 || ir4 > IRP_4.sensorAvg*0.3 ){ - // // almost to the end - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(1); - // wait_ms(200); - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // wait_ms(200); - // redLed.write(1); - // greenLed.write(0); - // blueLed.write(1); - // wait_ms(200); - // redLed.write(0); - // greenLed.write(1); - // blueLed.write(1); - - // }else if((ir3 < IRP_3.sensorAvg/(averageDivR)) && (ir2 < IRP_2.sensorAvg/(averageDivL)) ){ - // // both sides gone - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(1); - // wait_ms(100); - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // wait_ms(200); - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // wait_ms(200); - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // wait_ms(200); - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // }else if (ir3 < IRP_3.sensorAvg/averageDivR){// right wall gone - // // RED RED RED RED RED - // redLed.write(0); - // greenLed.write(1); - // blueLed.write(1); - // }else if (ir2 < IRP_2.sensorAvg/averageDivL){// left wall gone - // // BLUE BLUE BLUE BLUE - // redLed.write(1); - // greenLed.write(1); - // blueLed.write(0); - // }else if ((ir2 > ((IRP_2.sensorAvg/initAverageL)*averageDivUpper)) && (ir3 > ((IRP_3.sensorAvg/initAverageR)*averageDivUpper))){ - // // both walls there - // redLed.write(1); - // greenLed.write(0); - // blueLed.write(1); - // } - - - - - nCellEncoderAndIR(1); - wait_ms(1000); - - + // nCellEncoderAndIR(1); + // wait_ms(500); // turnRight(); // wait_ms(500); // nCellEncoderAndIR(1); @@ -945,9 +1192,7 @@ // nCellEncoderAndIR(5); // break; // turnRight180(); - - - + // int number = rand() % 4 + 1; // switch(number){ // case(1):{//turn right @@ -959,7 +1204,7 @@ // break; // } // case(3):{// keep going - + // break; // } // case(4):{// turnaround @@ -970,34 +1215,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; - //serial.printf("%f, %f \n", IRP_1.sensorAvg, IRP_4.sensorAvg); - //serial.printf("%f, %f \n", IRP_2.sensorAvg, IRP_3.sensorAvg); - //break; - - //serial.printf("IRS= >: %f, %f \r\n", IRP_2.getSamples(100), IRP_3.getSamples(100)); - //serial.printf("IRS= >: %f, %f \r\n", IRP_1.getSamples(100), IRP_4.getSamples(100)); - - - + /* counter2++; 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); @@ -1006,20 +1243,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); @@ -1035,8 +1269,8 @@ //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) ) ; //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); } -} +} \ No newline at end of file
--- a/main.h Sun May 21 08:52:43 2017 +0000 +++ b/main.h Sun May 21 09:58:54 2017 +0000 @@ -5,6 +5,9 @@ #include "ITG3200.h" #include "motor.h" #include "QEI.h" +#include <stack> // std::stack +#include <utility> // std::pair, std::make_pair + #define PULSES 3520 #define SAMPLE_NUM 100 @@ -72,6 +75,15 @@ void disableButton3(); void disableButton4(); +bool isWallInFront(int x, int y); +bool isWallInBack(int x, int y); +bool isWallOnRight(int x, int y); +bool isWallOnLeft(int x, int y); + +int chooseNextMovement(); +void changeManhattanDistance(bool headCenter); +bool hasVisited(int x, int y); + int dipFlags = 0; #define BUTTON1_FLAG 0x1 #define BUTTON2_FLAG 0x2 @@ -90,10 +102,20 @@ #define R_WALL 0x4 #define B_WALL 0x8 -int currDir = 100; // modulo this to keep track of the current direction of the mouse! +#define MAZE_LEN 16 + +int mouseX = 0; +int mouseY = 0; +bool justTurned = false; +bool goingToCenter = true; + +stack< pair<int, int> > cellsToVisit; + +int currDir = 102; // 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 mouseLoc[16][16] = {0}; // array to keep track of the mouse's current location -int manhattanDist[16][16] = { +int visitedCells[16][16] = {0}; // array to keep track of the mouse's current location +int manhattanDistances[16][16] = { {14, 13, 12, 11, 10, 9, 8, 7, 7, 8, 9, 10, 11, 12, 13, 14}, {13, 12, 11, 10, 9, 8, 7, 6, 6, 7, 8, 9, 10, 11, 12, 13}, {12, 11, 10, 9, 8, 7, 6, 5, 5, 6, 7, 8, 9, 10, 11, 12},