MacroRat
Mouse code for the MacroRat
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Tue Jul 12 2022 17:41:24 by
1.7.2