Mouse code for the MacroRat
main.cpp
- Committer:
- sahilmgandhi
- Date:
- 2017-05-28
- Revision:
- 40:465d2b565977
- Parent:
- 39:058fb32c24e0
- Child:
- 41:56a34315dd75
File content as of revision 40:465d2b565977:
#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 "ITG3200.h" #include "stm32f4xx.h" #include "QEI.h" /*LEFT/RIGHT BOTH WALLS 0.34 0.12 LEFT/RIGHT LEFT WALL GONE 0.02 0.11 LEFT/RIGHT RIGTH WALL GONE 0.33 0.008 LEFT/RIGHT BOTH WALLS GONE 0.02 0.008 LEFT/RIGHT CLOSE TO RIGHT WALL 0.14 0.47 LEFT/RIGHT CLOSE TO LEFT WALL 0.89 0.05 FRONT IRS NEAR WALL (STOPPING DISTANCE) 0.41 0.49 FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL) 0.07 0.06 */ #define IP_CONSTANT 2.00 #define II_CONSTANT 0.00001 #define ID_CONSTANT 0.2 void pidOnEncoders(); void moveForwardEncoder(double num) { int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int initial1 = count1; int initial0 = count0; int diff = count0 - count1; double kp = 0.00022; double kd = 0.00020; int prev = 0; double speed0 = WHEEL_SPEED+0.015; double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { //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; //double d = kp * x + kd * ( x - prev ); 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 left_motor.move( speed1-d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { left_motor.move( speed1-d ); right_motor.move( speed0+d ); } prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } //pidOnEncoders(); //pidBrake(); right_motor.brake(); left_motor.brake(); return; } void encoderAfterTurn(double num){ int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int initial1 = count1; int initial0 = count0; int diff = count0 - count1; double kp = 0.00022; double kd = 0.00020; int prev = 0; double speed0 = WHEEL_SPEED+0.015; double speed1 = WHEEL_SPEED; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); right_motor.move(speed0); left_motor.move(speed1); while( ((encoder0.getPulses() - initial0) <= (oneCellCountMomentum-270)*num && (encoder1.getPulses() - initial1) <= (oneCellCountMomentum-270)*num) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) { //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; //double d = kp * x + kd * ( x - prev ); 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 left_motor.move( speed1-d ); right_motor.move( speed0+d ); } else if( x > diff + 40 ) { left_motor.move( speed1-d ); right_motor.move( speed0+d ); } prev = x; receiverOneReading = IRP_1.getSamples(100); receiverFourReading = IRP_4.getSamples(100); } right_motor.brake(); left_motor.brake(); 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; } // the mouse has moved forward, we need to update the wall map now receiverOneReading = IRP_1.getSamples(100); receiverTwoReading = IRP_2.getSamples(100); 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); if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) { serial.printf("Front wall is there\n"); 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] |= B_WALL; } else if (currDir % 4 == 3) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; } } 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); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir % 4 == 1) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 2) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 3) { 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); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_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] |= F_WALL; } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { 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; } else if (currDir %4 == 1){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } else if (currDir % 4 == 2){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir %4 == 3){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } } } void printDipFlag() { if (DEBUGGING) serial.printf("Flag value is %d", dipFlags); } void enableButton1() { dipFlags |= BUTTON1_FLAG; printDipFlag(); } void enableButton2() { dipFlags |= BUTTON2_FLAG; printDipFlag(); } void enableButton3() { dipFlags |= BUTTON3_FLAG; printDipFlag(); } void enableButton4() { dipFlags |= BUTTON4_FLAG; printDipFlag(); } void disableButton1() { dipFlags &= ~BUTTON1_FLAG; printDipFlag(); } void disableButton2() { dipFlags &= ~BUTTON2_FLAG; printDipFlag(); } void disableButton3() { dipFlags &= ~BUTTON3_FLAG; printDipFlag(); } void disableButton4() { dipFlags &= ~BUTTON4_FLAG; printDipFlag(); } void pidOnEncoders() { int count0; int count1; count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int diff = count0 - count1; double kp = 0.00016; double kd = 0.00016; int prev = 0; int counter = 0; while(1) { count0 = encoder0.getPulses(); count1 = encoder1.getPulses(); int x = count0 - count1; //double d = kp * x + kd * ( x - prev ); 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 left_motor.move( -d ); right_motor.move( d ); } else if( x > diff + 60 ) { left_motor.move( -d ); right_motor.move( d ); } // else // { // left_motor.brake(); // right_motor.brake(); // } prev = x; counter++; if (counter == 10) break; } } void nCellEncoderAndIR(double cellCount) { double currentError = 0; double previousError = 0; double derivError = 0; double sumError = 0; double HIGH_PWM_VOLTAGE_R = WHEEL_SPEED; double HIGH_PWM_VOLTAGE_L = WHEEL_SPEED; double rightSpeed = WHEEL_SPEED; double leftSpeed = WHEEL_SPEED; int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount; int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount; left_motor.forward(WHEEL_SPEED); right_motor.forward(WHEEL_SPEED); float ir2 = IRP_2.getSamples( SAMPLE_NUM ); float ir3 = IRP_3.getSamples( SAMPLE_NUM ); int state = 0; while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) { // serial.printf("The desiredCount0 is: %d \t The desiredCount1 is: %d\t the 0encoderval is :%d\t the 1encoderval is : %d\t\n", desiredCount0, desiredCount1, encoder0.getPulses(), encoder1.getPulses()); receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM ); receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM ); receiverOneReading = IRP_1.getSamples( SAMPLE_NUM ); receiverFourReading = IRP_4.getSamples( SAMPLE_NUM ); // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg); if( receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) { break; } if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) { // both sides gone double prev0 = encoder0.getPulses(); double prev1 = encoder1.getPulses(); double diff0 = desiredCount0 - prev0; double diff1 = desiredCount1 - prev1; double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); redLed.write(1); greenLed.write(0); blueLed.write(0); // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass); moveForwardEncoder(valToPass); continue; } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone RED state = 1; // double prev0 = encoder0.getPulses(); // double prev1 = encoder1.getPulses(); // double diff0 = desiredCount0 - prev0; // double diff1 = desiredCount1 - prev1; // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); redLed.write(0); greenLed.write(1); blueLed.write(1); // moveForwardEncoder(valToPass); // break; } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone // BLUE BLUE BLUE BLUE state = 2; // double prev0 = encoder0.getPulses(); // double prev1 = encoder1.getPulses(); // double diff0 = desiredCount0 - prev0; // double diff1 = desiredCount1 - prev1; // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum); redLed.write(1); greenLed.write(1); blueLed.write(0); // moveForwardEncoder(valToPass); // break; } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { // both walls there state = 0; redLed.write(1); greenLed.write(0); blueLed.write(1); } //serial.printf("Entering switch\n"); switch(state) { case(0): { // both walls there currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg); break; } case(1): { // RED RED RED RED RED currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001); break; } case(2): { // blue currentError = (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg); break; } default: { currentError = ( receiverTwoReading - IRP_2.sensorAvg) - ( receiverThreeReading - IRP_3.sensorAvg); break; } } //serial.printf("Exiting switch"); sumError += currentError; derivError = currentError - previousError; double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError; double prev0 = encoder0.getPulses(); double prev1 = encoder1.getPulses(); double diff0 = desiredCount0 - prev0; double diff1 = desiredCount1 - prev1; double kp = .1/1000; rightSpeed = HIGH_PWM_VOLTAGE_R; leftSpeed = HIGH_PWM_VOLTAGE_L; if (diff1 < 1000 || diff0 < 1000) { rightSpeed = kp * HIGH_PWM_VOLTAGE_R; leftSpeed = kp * HIGH_PWM_VOLTAGE_L; } if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R); leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L); } else { // r is faster than L. speed up l, slow down r rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R); leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L); } //serial.printf("%f, %f\n", leftSpeed, rightSpeed); 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; } // GO BACK A BIT BEFORE BRAKING?? left_motor.backward(0.01); right_motor.backward(0.01); wait_us(150); // DELETE THIS IF IT DOES NOT WORK!! left_motor.brake(); right_motor.brake(); if (encoder0.getPulses() >= 0.5*desiredCount0 && encoder1.getPulses() >= 0.5*desiredCount1) { 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; } // the mouse has moved forward, we need to update the wall map now receiverOneReading = IRP_1.getSamples(100); receiverTwoReading = IRP_2.getSamples(100); 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); if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) { serial.printf("Front wall is there\n"); 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] |= B_WALL; } else if (currDir % 4 == 3) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL; } } 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"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir % 4 == 1) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL; } else if (currDir % 4 == 2) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 3) { 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"); if (currDir % 4 == 0) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL; } else if (currDir % 4 == 1) { wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_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] |= F_WALL; } } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) { 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; } else if (currDir %4 == 1){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } else if (currDir % 4 == 2){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL; } else if (currDir %4 == 3){ wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL; wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL; } } } } bool isWallInFront(int x, int y) { return (wallArray[MAZE_LEN - y - 1][x] & F_WALL); } bool isWallInBack(int x, int y) { return (wallArray[MAZE_LEN - y - 1][x] & B_WALL); } bool isWallOnRight(int x, int y) { return (wallArray[MAZE_LEN - y - 1][x] & R_WALL); } bool isWallOnLeft(int x, int y) { return (wallArray[MAZE_LEN - y - 1][x] & L_WALL); } 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(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( currX, currY)) { if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) { minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX]; } } if (currY - 1 >= 0 && !isWallInBack(currX, 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) { 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]; serial.printf("Looks like the left wall was not there\n"); 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; i++) { for (int j = 0; j < MAZE_LEN; j++) { distanceToCenter[i][j] = manhattanDistances[i][j]; } } for (int i = 0; i < MAZE_LEN; i++) { for (int j = 0; j < MAZE_LEN; j++) { manhattanDistances[i][j] = distanceToStart[i][j]; } } } else { for (int i = 0; i < MAZE_LEN; i++) { for (int j = 0; j < MAZE_LEN; j++) { distanceToStart[i][j] = manhattanDistances[i][j]; } } for (int i = 0; i < MAZE_LEN; i++) { for (int j = 0; j < MAZE_LEN; j++) { manhattanDistances[i][j] = distanceToCenter[i][j]; } } } } 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 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 = 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); } } } void waitButton4() { //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG); int init_val = dipFlags & BUTTON4_FLAG; while( (dipFlags & BUTTON4_FLAG) == init_val ) { // do nothing until ready } //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG); wait( 3 ); } int main() { //Set highest bandwidth. //gyro.setLpBandwidth(LPFBW_42HZ); 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 // Now TMR: enable clock with timer, APB1ENR // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN // // Encoder mode: disable timer before changing timer to encoder // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use // CCMR sets sample rate and set the channel to input // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels // 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); //waitButton4(); // init the wall, and mouse loc arrays: wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE; visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1; int prevEncoder0Count = 0; int prevEncoder1Count = 0; int currEncoder0Count = 0; int currEncoder1Count = 0; bool overrideFloodFill = false; right_motor.forward( WHEEL_SPEED ); left_motor.forward( WHEEL_SPEED ); //turn180(); //wait_ms(1500); int nextMovement = 0; // moveForwardEncoder(1); while (1) { // break; // prevEncoder0Count = encoder0.getPulses(); // prevEncoder1Count = encoder1.getPulses(); if (!overrideFloodFill){ nextMovement = chooseNextMovement(); // serial.printf("MouseX: %d, MouseY: %d\n", mouseX, mouseY); if (nextMovement == 0){ // serial.printf("Just Turned, want to go forward now\n"); redLed.write(1); greenLed.write(0); blueLed.write(1); encoderAfterTurn(1); // 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; turn180(); } } wait_ms(500); // reduce this once we fine tune this! //////////////////////// TESTING CODE GOES BELOW /////////////////////////// // encoderAfterTurn(1); // waitButton4(); // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\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 ); } }