main.cpp

00001 #include "mbed.h"
00002 
00003 #include "irpair.h"
00004 #include "main.h"
00005 #include "motor.h"
00006 
00007 #include <stdlib.h>
00008 #include <stack>          // std::stack
00009 #include <utility>      // std::pair, std::make_pair
00010 
00011 #include "ITG3200.h"
00012 #include "stm32f4xx.h"
00013 #include "QEI.h"
00014 
00015 /*LEFT/RIGHT BOTH WALLS
00016 0.34        0.12
00017 
00018 LEFT/RIGHT LEFT WALL GONE
00019 0.02        0.11
00020 
00021 LEFT/RIGHT RIGTH WALL GONE
00022 0.33        0.008
00023 
00024 LEFT/RIGHT BOTH WALLS GONE
00025 0.02        0.008
00026 
00027 LEFT/RIGHT CLOSE TO RIGHT WALL
00028 0.14        0.47
00029 
00030 LEFT/RIGHT CLOSE TO LEFT WALL
00031 0.89        0.05
00032 
00033 FRONT IRS NEAR WALL (STOPPING DISTANCE)
00034 0.41        0.49
00035 
00036 FRONT IRS NO WALL AHEAD (FOR ATLEAST 1 FULL CELL)
00037 0.07        0.06
00038 
00039 */
00040 
00041 #define IP_CONSTANT 1.6
00042 #define II_CONSTANT 0.00001
00043 #define ID_CONSTANT 0.24
00044 
00045 void pidOnEncoders();
00046 
00047 void moveBackwards(){
00048     right_motor.move(-WHEEL_SPEED);
00049     left_motor.move(-WHEEL_SPEED);
00050     wait_ms(40);
00051     right_motor.brake();
00052     left_motor.brake();
00053 }
00054 
00055 void moveForwardEncoder(double num)
00056 {
00057     int count0;
00058     int count1;
00059     count0 = encoder0.getPulses();
00060     count1 = encoder1.getPulses();
00061     int initial1 = count1;
00062     int initial0 = count0;
00063     int diff = count0 - count1;
00064     double kp = 0.00022;
00065     double kd = 0.00020;
00066     int prev = 0;
00067 
00068     double speed0 = WHEEL_SPEED;
00069     double speed1 = WHEEL_SPEED;
00070 
00071     receiverOneReading = IRP_1.getSamples(100);
00072     receiverFourReading = IRP_4.getSamples(100);
00073     right_motor.move(speed0);
00074     left_motor.move(speed1);
00075     wait_ms(150);
00076 
00077     double distance_to_go = (oneCellCount)*num;
00078 
00079     while(  ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {
00080 
00081         if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
00082             left_motor.move( speed0/900 );
00083             right_motor.move( speed1/900 );
00084         } else {
00085             left_motor.move(speed0);
00086             right_motor.move(speed1);
00087             wait_us(16000);
00088             pidOnEncoders();
00089         }
00090 
00091         receiverOneReading = IRP_1.getSamples(100);
00092         receiverFourReading = IRP_4.getSamples(100);
00093     }
00094 
00095     right_motor.brake();
00096     left_motor.brake();
00097     return;
00098 }
00099 
00100 void encoderAfterTurn(double num)
00101 {
00102     int count0;
00103     int count1;
00104     count0 = encoder0.getPulses();
00105     count1 = encoder1.getPulses();
00106     int initial1 = count1;
00107     int initial0 = count0;
00108     int diff = count0 - count1;
00109     double kp = 0.00008;
00110     double kd = 0.0000;
00111     int prev = 0;
00112 
00113     double speed0 = WHEEL_SPEED; // left wheel
00114     double speed1 = WHEEL_SPEED;
00115     receiverOneReading = IRP_1.getSamples(100);
00116     receiverFourReading = IRP_4.getSamples(100);
00117 
00118     right_motor.move(speed0);
00119     left_motor.move(speed1);
00120     wait_us(150000);
00121 
00122     double distance_to_go = (oneCellCount)*num;
00123 
00124     while(  ((encoder0.getPulses() - initial0) <= distance_to_go && (encoder1.getPulses() - initial1) <= distance_to_go) && receiverOneReading < IRP_1.sensorAvg*frontStop && receiverFourReading < IRP_4.sensorAvg*frontStop) {
00125 
00126         if( ( distance_to_go - count0 ) <= 900 || (distance_to_go - count1) <= 900 ) {
00127             left_motor.move( speed0/900 );
00128             right_motor.move( speed1/900 );
00129         } else {
00130             left_motor.move(speed0);
00131             right_motor.move(speed1);
00132             wait_us(16000);
00133             pidOnEncoders();
00134         }
00135         receiverOneReading = IRP_1.getSamples(100);
00136         receiverFourReading = IRP_4.getSamples(100);
00137     }
00138 
00139     right_motor.brake();
00140     left_motor.brake();
00141 
00142     double curr0 = encoder0.getPulses();
00143     double curr1 = encoder1.getPulses();
00144     if ((curr0 - initial0) >= distance_to_go*0.6 && (curr1 - initial1) >= distance_to_go*0.6) {
00145         if (currDir % 4 == 0) {
00146             mouseY += 1;
00147         } else if (currDir % 4 == 1) {
00148             mouseX += 1;
00149         } else if (currDir % 4 == 2) {
00150             mouseY -= 1;
00151         } else if (currDir % 4 == 3) {
00152             mouseX -= 1;
00153         }
00154 
00155         // the mouse has moved forward, we need to update the wall map now
00156         receiverOneReading = IRP_1.getSamples(100);
00157         receiverTwoReading = IRP_2.getSamples(100);
00158         receiverThreeReading = IRP_3.getSamples(100);
00159         receiverFourReading = IRP_4.getSamples(100);
00160 
00161         // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
00162         // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
00163 
00164         if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
00165             // serial.printf("Front wall is there\n");
00166             if (currDir % 4 == 0) {
00167                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00168             } else if (currDir % 4 == 1) {
00169                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00170             } else if (currDir % 4 == 2) {
00171                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00172             } else if (currDir % 4 == 3) {
00173                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
00174             }
00175         }
00176         if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
00177             // do nothing, the walls are not there
00178         } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
00179             // serial.printf("Left wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
00180             if (currDir % 4 == 0) {
00181                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00182             } else if (currDir % 4 == 1) {
00183                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00184             } else if (currDir % 4 == 2) {
00185                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00186             } else if (currDir % 4 == 3) {
00187                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00188             }
00189         } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
00190             // serial.printf("Right wall, mouseY: %d, mouseX: %d\n", mouseY, mouseX);
00191             if (currDir % 4 == 0) {
00192                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00193             } else if (currDir % 4 == 1) {
00194                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00195             } else if (currDir % 4 == 2) {
00196                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
00197             } else if (currDir % 4 == 3) {
00198                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00199             }
00200         } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
00201             // serial.printf("Both walls \n");
00202             if (currDir %4 == 0) {
00203                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
00204                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00205             } else if (currDir %4 == 1) {
00206                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
00207                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
00208             } else if (currDir % 4 == 2) {
00209                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
00210                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00211             } else if (currDir %4 == 3) {
00212                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
00213                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
00214             }
00215         }
00216     }
00217 }
00218 
00219 void printDipFlag()
00220 {
00221     if (DEBUGGING)
00222         serial.printf("Flag value is %d", dipFlags);
00223 }
00224 
00225 void enableButton1()
00226 {
00227     dipFlags |= BUTTON1_FLAG;
00228     printDipFlag();
00229 }
00230 void enableButton2()
00231 {
00232     dipFlags |= BUTTON2_FLAG;
00233     printDipFlag();
00234 }
00235 void enableButton3()
00236 {
00237     dipFlags |= BUTTON3_FLAG;
00238     printDipFlag();
00239 }
00240 void enableButton4()
00241 {
00242     dipFlags |= BUTTON4_FLAG;
00243     printDipFlag();
00244 }
00245 void disableButton1()
00246 {
00247     dipFlags &= ~BUTTON1_FLAG;
00248     printDipFlag();
00249 }
00250 void disableButton2()
00251 {
00252     dipFlags &= ~BUTTON2_FLAG;
00253     printDipFlag();
00254 }
00255 void disableButton3()
00256 {
00257     dipFlags &= ~BUTTON3_FLAG;
00258     printDipFlag();
00259 }
00260 void disableButton4()
00261 {
00262     dipFlags &= ~BUTTON4_FLAG;
00263     printDipFlag();
00264 }
00265 
00266 void pidOnEncoders()
00267 {
00268     int count0;
00269     int count1;
00270     count0 = encoder0.getPulses();
00271     count1 = encoder1.getPulses();
00272     int diff = count0 - count1;
00273     double kp = 0.00016;
00274     double kd = 0.00016;
00275     int prev = 0;
00276 
00277     int counter = 0;
00278     while(1) {
00279         count0 = encoder0.getPulses();
00280         count1 = encoder1.getPulses();
00281         int x = count0 - count1;
00282         //double d = kp * x + kd * ( x - prev );
00283         double kppart = kp * x;
00284         double kdpart = kd * (x-prev);
00285         double d = kppart + kdpart;
00286 
00287         //serial.printf( "x: %d,\t prev: %d,\t d: %f,\t kppart: %f,\t kdpart: %f\n", x, prev, d, kppart, kdpart );
00288         if( x < diff - 60 ) { // count1 is bigger, right wheel pushed forward
00289             left_motor.move( -d );
00290             right_motor.move( d );
00291         } else if( x > diff + 60 ) {
00292             left_motor.move( -d );
00293             right_motor.move( d );
00294         }
00295         // else
00296         // {
00297         //     left_motor.brake();
00298         //     right_motor.brake();
00299         // }
00300         prev = x;
00301         counter++;
00302         if (counter == 10)
00303             break;
00304     }
00305 }
00306 
00307 void nCellEncoderAndIR(double cellCount)
00308 {
00309     double currentError = 0;
00310     double previousError = 0;
00311     double derivError = 0;
00312     double sumError = 0;
00313 
00314     double HIGH_PWM_VOLTAGE_R = WHEEL_SPEED;
00315     double HIGH_PWM_VOLTAGE_L = WHEEL_SPEED;
00316 
00317     double rightSpeed = WHEEL_SPEED;
00318     double leftSpeed = WHEEL_SPEED;
00319 
00320     int desiredCount0 = encoder0.getPulses() + oneCellCountMomentum*cellCount;
00321     int desiredCount1 = encoder1.getPulses() + oneCellCountMomentum*cellCount;
00322 
00323     left_motor.forward(WHEEL_SPEED);
00324     right_motor.forward(WHEEL_SPEED);
00325 
00326     float ir2 = IRP_2.getSamples( SAMPLE_NUM );
00327     float ir3 = IRP_3.getSamples( SAMPLE_NUM );
00328 
00329     int state = 0;
00330 
00331     int previousCount0 = 10000;
00332     int previousCount1 = 10000;
00333 
00334     while (encoder0.getPulses() <= desiredCount0 && encoder1.getPulses() <= desiredCount1) {
00335         int currCount0 = encoder0.getPulses();
00336         int currCount1 = encoder1.getPulses();
00337         // serial.printf("currCount0 = %d, currCount1 =%d, previousCount0 = %d, previousCount1 = %d \n", currCount0, currCount1, previousCount0, previousCount1);
00338         // if (abs(currCount0 - previousCount0) <= 2 || abs(currCount1 - previousCount1) <= 2){
00339         //     moveBackwards();
00340         //     return;
00341         // }
00342         // 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());
00343         receiverTwoReading = IRP_2.getSamples( SAMPLE_NUM );
00344         receiverThreeReading = IRP_3.getSamples( SAMPLE_NUM );
00345         receiverOneReading = IRP_1.getSamples( SAMPLE_NUM );
00346         receiverFourReading = IRP_4.getSamples( SAMPLE_NUM );
00347         // serial.printf("IR2 = %f, IR2AVE = %f, IR3 = %f, IR3_AVE = %f\n", receiverTwoReading, IRP_2.sensorAvg, receiverThreeReading, IRP_3.sensorAvg);
00348         if(  receiverOneReading > IRP_1.sensorAvg * frontStop || receiverFourReading > IRP_4.sensorAvg * frontStop ) {
00349             break;
00350         }
00351 
00352         if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
00353             // both sides gone
00354             double prev0 = encoder0.getPulses();
00355             double prev1 = encoder1.getPulses();
00356             double diff0 = desiredCount0 - prev0;
00357             double diff1 = desiredCount1 - prev1;
00358             double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
00359             redLed.write(1);
00360             greenLed.write(0);
00361             blueLed.write(0);
00362             // serial.printf("Going to go over to move forward with encoder, and passing %f\n", valToPass);
00363             moveForwardEncoder(valToPass);
00364             continue;
00365         } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
00366             state = 1;
00367             // double prev0 = encoder0.getPulses();
00368             // double prev1 = encoder1.getPulses();
00369             // double diff0 = desiredCount0 - prev0;
00370             // double diff1 = desiredCount1 - prev1;
00371             // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
00372             redLed.write(0);
00373             greenLed.write(1);
00374             blueLed.write(1);
00375             // moveForwardEncoder(valToPass);
00376             // break;
00377         } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
00378             // BLUE BLUE BLUE BLUE
00379             state = 2;
00380             // double prev0 = encoder0.getPulses();
00381             // double prev1 = encoder1.getPulses();
00382             // double diff0 = desiredCount0 - prev0;
00383             // double diff1 = desiredCount1 - prev1;
00384             // double valToPass = ((diff0 + diff1)/2)/(oneCellCountMomentum);
00385             redLed.write(1);
00386             greenLed.write(1);
00387             blueLed.write(0);
00388             // moveForwardEncoder(valToPass);
00389             // break;
00390         } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
00391             // both walls there
00392             state = 0;
00393             redLed.write(1);
00394             greenLed.write(0);
00395             blueLed.write(1);
00396         }
00397 
00398         //serial.printf("Entering switch\n");
00399         switch(state) {
00400             case(0): { // both walls there
00401                 currentError = ( receiverTwoReading - IRP_2.sensorAvg)  - ( receiverThreeReading - IRP_3.sensorAvg);
00402                 break;
00403             }
00404             case(1): { // RED RED RED RED RED
00405                 currentError = (receiverTwoReading - IRP_2.sensorAvg) - (IRP_3.sensorAvg*0.001);
00406                 break;
00407             }
00408             case(2): { // blue
00409                 currentError =  (IRP_2.sensorAvg*0.001) - (receiverThreeReading - IRP_3.sensorAvg);
00410                 break;
00411             }
00412             default: {
00413                 currentError = ( receiverTwoReading - IRP_2.sensorAvg)  - ( receiverThreeReading - IRP_3.sensorAvg);
00414                 break;
00415             }
00416         }
00417         //serial.printf("Exiting switch");
00418 
00419         sumError += currentError;
00420         derivError = currentError - previousError;
00421         double PIDSum = IP_CONSTANT*currentError + II_CONSTANT*sumError + ID_CONSTANT*derivError;
00422 
00423         double prev0 = encoder0.getPulses();
00424         double prev1 = encoder1.getPulses();
00425         double diff0 = desiredCount0 - prev0;
00426         double diff1 = desiredCount1 - prev1;
00427         double kp = .1/1000;
00428 
00429 
00430         rightSpeed = HIGH_PWM_VOLTAGE_R;
00431         leftSpeed = HIGH_PWM_VOLTAGE_L;
00432         if (diff1 < 1000 || diff0 < 1000) {
00433             rightSpeed = kp * HIGH_PWM_VOLTAGE_R;
00434             leftSpeed = kp * HIGH_PWM_VOLTAGE_L;
00435         }
00436 
00437         if (PIDSum > 0) { // this means the leftWheel is faster than the right. So right speeds up, left slows down
00438             rightSpeed = rightSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_R);
00439             leftSpeed = leftSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_L);
00440         } else { // r is faster than L. speed up l, slow down r
00441             rightSpeed = rightSpeed + abs(PIDSum*HIGH_PWM_VOLTAGE_R);
00442             leftSpeed = leftSpeed - abs(PIDSum*HIGH_PWM_VOLTAGE_L);
00443         }
00444 
00445 
00446         //serial.printf("%f, %f\n", leftSpeed, rightSpeed);
00447         if (leftSpeed > 0.30) leftSpeed = 0.30;
00448         if (leftSpeed < 0) leftSpeed = 0;
00449         if (rightSpeed > 0.30) rightSpeed = 0.30;
00450         if (rightSpeed < 0) rightSpeed = 0;
00451 
00452         right_motor.forward(rightSpeed);
00453         left_motor.forward(leftSpeed);
00454         pidOnEncoders();
00455 
00456         previousError = currentError;
00457         previousCount1 = currCount1;
00458         previousCount0 = currCount0;
00459     }
00460 
00461     // GO BACK A BIT BEFORE BRAKING??
00462     left_motor.backward(0.01);
00463     right_motor.backward(0.01);
00464     wait_us(150);
00465     // DELETE THIS IF IT DOES NOT WORK!!
00466 
00467     left_motor.brake();
00468     right_motor.brake();
00469     if (encoder0.getPulses() >= 0.4*desiredCount0 && encoder1.getPulses() >= 0.4*desiredCount1) {
00470         if (currDir % 4 == 0) {
00471             mouseY += 1;
00472         } else if (currDir % 4 == 1) {
00473             mouseX += 1;
00474         } else if (currDir % 4 == 2) {
00475             mouseY -= 1;
00476         } else if (currDir % 4 == 3) {
00477             mouseX -= 1;
00478         }
00479 
00480         // the mouse has moved forward, we need to update the wall map now
00481         receiverOneReading = IRP_1.getSamples(100);
00482         receiverTwoReading = IRP_2.getSamples(100);
00483         receiverThreeReading = IRP_3.getSamples(100);
00484         receiverFourReading = IRP_4.getSamples(100);
00485 
00486         // serial.printf("R1: %f \t R4: %f \t R1Avg: %f \t R4Avg: %f\n", receiverOneReading, receiverFourReading, IRP_1.sensorAvg, IRP_4.sensorAvg);
00487         // serial.printf("R2: %f \t R3: %f \t R2Avg: %f \t R3Avg: %f\n", receiverTwoReading, receiverThreeReading, IRP_2.sensorAvg, IRP_3.sensorAvg);
00488 
00489         if (receiverOneReading >= IRP_1.sensorAvg * 2.5 || receiverFourReading >= IRP_4.sensorAvg * 2.5) {
00490             // serial.printf("Front wall is there\n");
00491             if (currDir % 4 == 0) {
00492                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00493             } else if (currDir % 4 == 1) {
00494                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00495             } else if (currDir % 4 == 2) {
00496                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00497             } else if (currDir % 4 == 3) {
00498                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
00499             }
00500         }
00501         if((receiverThreeReading < IRP_3.sensorAvg/5) && (receiverTwoReading < IRP_2.sensorAvg/5)) {
00502             // do nothing, the walls are not there
00503         } else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone, left wall is there   RED
00504             // serial.printf("Left wall\n");
00505             if (currDir % 4 == 0) {
00506                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00507             } else if (currDir % 4 == 1) {
00508                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00509             } else if (currDir % 4 == 2) {
00510                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00511             } else if (currDir % 4 == 3) {
00512                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00513             }
00514         } else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone, right wall is there
00515             // serial.printf("Right wall\n");
00516             if (currDir % 4 == 0) {
00517                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= R_WALL;
00518             } else if (currDir % 4 == 1) {
00519                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= B_WALL;
00520             } else if (currDir % 4 == 2) {
00521                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= L_WALL;
00522             } else if (currDir % 4 == 3) {
00523                 wallArray[MAZE_LEN - 1 - (mouseY)][mouseX] |= F_WALL;
00524             }
00525         } else if ((receiverTwoReading > ((IRP_2.sensorAvg)*averageDivUpper)) && (receiverThreeReading > ((IRP_3.sensorAvg)*averageDivUpper))) {
00526             // serial.printf("Both walls \n");
00527             if (currDir %4 == 0) {
00528                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
00529                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00530             } else if (currDir %4 == 1) {
00531                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
00532                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
00533             } else if (currDir % 4 == 2) {
00534                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= R_WALL;
00535                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= L_WALL;
00536             } else if (currDir %4 == 3) {
00537                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= F_WALL;
00538                 wallArray[MAZE_LEN - 1 - mouseY][mouseX] |= B_WALL;
00539             }
00540         }
00541     }
00542 }
00543 
00544 bool isWallInFront(int x, int y)
00545 {
00546     return (wallArray[MAZE_LEN - y - 1][x] & F_WALL);
00547 }
00548 bool isWallInBack(int x, int y)
00549 {
00550     return (wallArray[MAZE_LEN - y - 1][x] & B_WALL);
00551 }
00552 bool isWallOnRight(int x, int y)
00553 {
00554     return (wallArray[MAZE_LEN - y - 1][x] & R_WALL);
00555 }
00556 bool isWallOnLeft(int x, int y)
00557 {
00558     return (wallArray[MAZE_LEN - y - 1][x] & L_WALL);
00559 }
00560 
00561 int chooseNextMovement()
00562 {
00563     int currentDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX];
00564     if (goingToCenter && (currentDistance == 0)) {
00565         goingToCenter = false;
00566         changeManhattanDistance(goingToCenter);
00567     } else if (!goingToCenter && (currentDistance == 0)) {
00568         goingToCenter = true;
00569         changeManhattanDistance(goingToCenter);
00570     }
00571 
00572     visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
00573     int currX = 0;
00574     int currY = 0;
00575     int currDist = 0;
00576     int dirToGo = 0;
00577     if (!justTurned) {
00578         cellsToVisit.push(make_pair(mouseX, mouseY));
00579         while (!cellsToVisit.empty()) {
00580             pair<int, int> curr = cellsToVisit.top();
00581             cellsToVisit.pop();
00582             currX = curr.first;
00583             currY = curr.second;
00584             currDist = manhattanDistances[(MAZE_LEN - 1) - currY][currX];
00585             int minDist = INT_MAX;
00586 
00587             if (hasVisited(currX, currY)) { // then we want to actually see where the walls are, else we treat it as if there are no walls!
00588                 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
00589                     if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
00590                         minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
00591                     }
00592                 }
00593                 if (currX - 1 >= 0 && !isWallOnLeft(currX, currY)) {
00594                     if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
00595                         minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
00596                     }
00597                 }
00598                 if (currY + 1 < MAZE_LEN && !isWallInFront( currX,  currY)) {
00599                     if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
00600                         minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
00601                     }
00602                 }
00603                 if (currY - 1 >= 0 && !isWallInBack(currX,  currY)) {
00604                     if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
00605                         minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
00606                     }
00607                 }
00608             } else {
00609                 if (currX + 1 < MAZE_LEN) {
00610                     if (manhattanDistances[MAZE_LEN - 1 - currY][currX + 1] < minDist) {
00611                         minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX + 1];
00612                     }
00613                 }
00614                 if (currX - 1 >= 0) {
00615                     if (manhattanDistances[MAZE_LEN - 1 - currY][currX - 1] < minDist) {
00616                         minDist = manhattanDistances[MAZE_LEN - 1 - currY][currX - 1];
00617                     }
00618                 }
00619                 if (currY + 1 < MAZE_LEN) {
00620                     if (manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX] < minDist) {
00621                         minDist = manhattanDistances[MAZE_LEN - 1 - (currY + 1)][currX];
00622                     }
00623                 }
00624                 if (currY - 1 >= 0) {
00625                     if (manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX] < minDist) {
00626                         minDist = manhattanDistances[MAZE_LEN - 1 - (currY - 1)][currX];
00627                     }
00628                 }
00629             }
00630 
00631             if (minDist == INT_MAX)
00632                 continue;
00633             if (currDist > minDist)
00634                 continue;
00635             if (currDist <= minDist) {
00636                 manhattanDistances[MAZE_LEN - 1 - currY][currX] = minDist + 1;
00637             }
00638             if (hasVisited(currX, currY)) {
00639                 if (currY + 1 < MAZE_LEN && !isWallInFront(currX, currY)) {
00640                     cellsToVisit.push(make_pair(currX, currY + 1));
00641                 }
00642                 if (currX + 1 < MAZE_LEN && !isWallOnRight(currX, currY)) {
00643                     cellsToVisit.push(make_pair(currX + 1, currY));
00644                 }
00645                 if (currY - 1 >= 0 && !isWallInBack(currX, currY)) {
00646                     cellsToVisit.push(make_pair(currX, currY - 1));
00647                 }
00648                 if (currX - 1 >= 0 && !isWallOnLeft( currX, currY)) {
00649                     cellsToVisit.push(make_pair(currX - 1, currY));
00650                 }
00651             } else {
00652                 if (currY + 1 < MAZE_LEN) {
00653                     cellsToVisit.push(make_pair(currX, currY + 1));
00654                 }
00655                 if (currX + 1 < MAZE_LEN) {
00656                     cellsToVisit.push(make_pair(currX + 1, currY));
00657                 }
00658                 if (currY - 1 >= 0) {
00659                     cellsToVisit.push(make_pair(currX, currY - 1));
00660                 }
00661                 if (currX - 1 >= 0) {
00662                     cellsToVisit.push(make_pair(currX - 1, currY));
00663                 }
00664             }
00665         }
00666 
00667         int minDistance = INT_MAX;
00668         if (currDir % 4 == 0) {
00669             if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
00670                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
00671                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
00672                     dirToGo = 1;
00673                 }
00674             }
00675             if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
00676                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
00677                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
00678                     dirToGo = 2;
00679                 }
00680             }
00681             if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
00682                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
00683                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
00684                     dirToGo = 3;
00685                 }
00686             }
00687             if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
00688                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
00689                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
00690                     dirToGo = 4;
00691                 }
00692             }
00693         } else if (currDir % 4 == 2) {
00694             if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
00695                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
00696                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1];
00697                     dirToGo = 1;
00698                 }
00699             }
00700             if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
00701                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
00702                     minDistance = manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1];
00703                     dirToGo = 2;
00704                 }
00705             }
00706             if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
00707                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
00708                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
00709                     dirToGo = 3;
00710                 }
00711             }
00712             if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
00713                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
00714                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
00715                     dirToGo = 4;
00716                 }
00717             }
00718         } else if (currDir % 4 == 1) {
00719             if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
00720                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
00721                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
00722                     dirToGo = 1;
00723                 }
00724             }
00725             if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
00726                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
00727                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
00728                     // serial.printf("Looks like the left wall was not there\n");
00729                     dirToGo = 2;
00730                 }
00731             }
00732             if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
00733                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX + 1] <= minDistance) {
00734                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
00735                     dirToGo = 3;
00736                 }
00737             }
00738             if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
00739                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1] <= minDistance) {
00740                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
00741                     dirToGo = 4;
00742                 }
00743             }
00744         } else if (currDir % 4 == 3) {
00745             if (mouseY + 1 < MAZE_LEN && !isWallInFront(mouseX, mouseY)) {
00746                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX] <= minDistance) {
00747                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY + 1)][mouseX];
00748                     dirToGo = 1;
00749                 }
00750             }
00751             if (mouseY - 1 >= 0 && !isWallInBack(mouseX, mouseY)) {
00752                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX] <= minDistance) {
00753                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY - 1)][mouseX];
00754                     dirToGo = 2;
00755                 }
00756             }
00757             if (mouseX - 1 >= 0 && !isWallOnLeft(mouseX, mouseY)) {
00758                 if (manhattanDistances[MAZE_LEN - 1 - mouseY][mouseX - 1] <= minDistance) {
00759                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX - 1];
00760                     dirToGo = 3;
00761                 }
00762             }
00763             if (mouseX + 1 < MAZE_LEN && !isWallOnRight(mouseX, mouseY)) {
00764                 if (manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1] <= minDistance) {
00765                     minDistance = manhattanDistances[MAZE_LEN - 1 - (mouseY)][mouseX + 1];
00766                     dirToGo = 4;
00767                 }
00768             }
00769         }
00770     } else {
00771         justTurned = false;
00772         dirToGo = 0;
00773     }
00774 
00775     return dirToGo;
00776 }
00777 
00778 bool hasVisited(int x, int y)
00779 {
00780     return visitedCells[MAZE_LEN - 1 - y][x];
00781 }
00782 
00783 void changeManhattanDistance(bool headCenter)
00784 {
00785     if (headCenter) {
00786         for (int i = 0; i < MAZE_LEN; i++) {
00787             for (int j = 0; j < MAZE_LEN; j++) {
00788                 distanceToCenter[i][j] = manhattanDistances[i][j];
00789             }
00790         }
00791         for (int i = 0; i < MAZE_LEN; i++) {
00792             for (int j = 0; j < MAZE_LEN; j++) {
00793                 manhattanDistances[i][j] = distanceToStart[i][j];
00794             }
00795         }
00796     } else {
00797         for (int i = 0; i < MAZE_LEN; i++) {
00798             for (int j = 0; j < MAZE_LEN; j++) {
00799                 distanceToStart[i][j] = manhattanDistances[i][j];
00800             }
00801         }
00802         for (int i = 0; i < MAZE_LEN; i++) {
00803             for (int j = 0; j < MAZE_LEN; j++) {
00804                 manhattanDistances[i][j] = distanceToCenter[i][j];
00805             }
00806         }
00807     }
00808 }
00809 
00810 void initializeDistances()
00811 {
00812     for (int i = 0; i < MAZE_LEN / 2; i++) {
00813         for (int j = 0; j < MAZE_LEN / 2; j++) {
00814             distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
00815         }
00816     }
00817     for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
00818         for (int j = 0; j < MAZE_LEN / 2; j++) {
00819             distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
00820         }
00821     }
00822     for (int i = 0; i < MAZE_LEN / 2; i++) {
00823         for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
00824             distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
00825         }
00826     }
00827     for (int i = MAZE_LEN / 2; i < MAZE_LEN; i++) {
00828         for (int j = MAZE_LEN / 2; j < MAZE_LEN; j++) {
00829             distanceToStart[MAZE_LEN - 1 - j][i] = abs(0 - j) + abs(0 - i);
00830         }
00831     }
00832 }
00833 
00834 void waitButton4()
00835 {
00836     //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
00837     int init_val = dipFlags & BUTTON4_FLAG;
00838     while( (dipFlags & BUTTON4_FLAG) == init_val ) {
00839         // do nothing until ready
00840     }
00841     //serial.printf("%d, %d, %d\n", dipFlags, BUTTON1_FLAG, dipFlags & BUTTON1_FLAG);
00842     wait( 3 );
00843 }
00844 
00845 int main()
00846 {
00847     //Set highest bandwidth.
00848     //gyro.setLpBandwidth(LPFBW_42HZ);
00849     initializeDistances();
00850     serial.baud(9600);
00851     //serial.printf("The gyro's address is %s", gyro.getWhoAmI());
00852 
00853     wait (0.1);
00854 
00855     redLed.write(1);
00856     greenLed.write(0);
00857     blueLed.write(1);
00858 
00859     //left_motor.forward(0.1);
00860     //right_motor.forward(0.1);
00861 
00862     // PA_1 is A of right
00863     // PA_0 is B of right
00864     // PA_5 is A of left
00865     // PB_3 is B of left
00866     //QEI encoder0( PA_5, PB_3, NC, PULSES, QEI::X4_ENCODING );
00867     //    QEI encoder1( PA_1, PA_0, NC, PULSES, QEI::X4_ENCODING );
00868 
00869     // TODO: Setting all the registers and what not for Quadrature Encoders
00870     /*    RCC->APB1ENR |= 0x1001; // Enable clock for Tim2 (Bit 0) and Tim5 (Bit 3)
00871         RCC->AHB1ENR |= 0x11; // Enable GPIO port clock enables for Tim2(A) and Tim5(B)
00872         GPIOA->AFR[0] |= 0x10; // Set GPIO alternate function modes for Tim2
00873         GPIOB->AFR[0] |= 0x100; // Set GPIO alternate function modes for Tim5
00874         */
00875 
00876     // 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
00877     // of alternate function specified
00878     // 4 modes sets AHB1ENR
00879     // Now TMR: enable clock with timer, APB1ENR
00880     // set period, autoreload value, ARR value 2^32-1, CR1 - TMR resets itself, ARPE and EN
00881     //
00882     // Encoder mode: disable timer before changing timer to encoder
00883     // CCMR1/2 1/2 depends on channel 1/2 or 3/4, depends on upper bits, depending which channels you use
00884     // CCMR sets sample rate and set the channel to input
00885     // CCER, which edge to trigger on, cannot be 11(not allowed for encoder mode), CCER for both channels
00886     // SMCR - encoder mode
00887     // CR1 reenabales
00888     // then read CNT reg of timer
00889 
00890     dipButton1.rise(&enableButton1);
00891     dipButton2.rise(&enableButton2);
00892     dipButton3.rise(&enableButton3);
00893     dipButton4.rise(&enableButton4);
00894 
00895     dipButton1.fall(&disableButton1);
00896     dipButton2.fall(&disableButton2);
00897     dipButton3.fall(&disableButton3);
00898     dipButton4.fall(&disableButton4);
00899 
00900     //waitButton4();
00901 
00902     // init the wall, and mouse loc arrays:
00903     wallArray[MAZE_LEN - 1 - mouseY][mouseX] = 0xE;
00904     visitedCells[MAZE_LEN - 1 - mouseY][mouseX] = 1;
00905 
00906     int prevEncoder0Count = 0;
00907     int prevEncoder1Count = 0;
00908     int currEncoder0Count = 0;
00909     int currEncoder1Count = 0;
00910 
00911     bool overrideFloodFill = false;
00912     right_motor.forward( WHEEL_SPEED );
00913     left_motor.forward( WHEEL_SPEED );
00914     //turn180();
00915     //wait_ms(1500);
00916     int nextMovement = 0;
00917 
00918     // moveForwardEncoder(1);
00919 
00920     while (1) {
00921         if (receiverOneReading < IRP_1.sensorAvg * frontStop && receiverFourReading < IRP_4.sensorAvg * frontStop ){
00922             nCellEncoderAndIR(1);
00923             continue;
00924         }
00925         else if (receiverThreeReading < IRP_3.sensorAvg/LRAvg) { // right wall gone   RED
00926             turnRight();
00927             wait_ms(100);
00928             moveForwardEncoder(0.4);
00929             nCellEncoderAndIR(0.6);
00930             continue;
00931         } 
00932         else if (receiverTwoReading < IRP_2.sensorAvg/LRAvg) { // left wall gone
00933             turnLeft();
00934             wait_ms(100);
00935             moveForwardEncoder(0.4);
00936             nCellEncoderAndIR(0.6);
00937             continue;
00938         }
00939         else{
00940             turn180();
00941             wait_ms(100);
00942             moveForwardEncoder(0.4);
00943             nCellEncoderAndIR(0.6);
00944             continue;
00945         }
00946 
00947         //////////////////////// TESTING CODE GOES BELOW ///////////////////////////
00948 
00949         // encoderAfterTurn(1);
00950         // waitButton4();
00951         // serial.printf("Encoder 0 is : %d \t Encoder 1 is %d\n",encoder0.getPulses(), encoder1.getPulses() );
00952         // double currentError = ( (IRP_2.getSamples( SAMPLE_NUM ) - IRP_2.sensorAvg) ) - ( (IRP_3.getSamples( SAMPLE_NUM ) - IRP_3.sensorAvg) ) ;
00953         // 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 );
00954         //encoderAfterTurn(1);
00955         //waitButton4();
00956     }
00957 }