James Allen
/
MyMazeSolver
working modified m3pimazesolver
main.cpp@2:73f7cc4c30c7, 2013-04-06 (annotated)
- Committer:
- jalle1714
- Date:
- Sat Apr 06 15:22:01 2013 +0000
- Revision:
- 2:73f7cc4c30c7
- Parent:
- 1:f4db9b0bb5e2
Added playtune to play a melody at the end of pressed function; Put in a one second pause after resistor pullup to prevent from jumping to pressed function before button pressed
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jalle1714 | 0:05dee1d3d857 | 1 | #include "mbed.h" |
jalle1714 | 0:05dee1d3d857 | 2 | #include "m3pimaze.h" |
jalle1714 | 0:05dee1d3d857 | 3 | |
jalle1714 | 0:05dee1d3d857 | 4 | BusOut leds(LED1,LED2,LED3,LED4); |
jalle1714 | 0:05dee1d3d857 | 5 | m3pi m3pi(p23,p9,p10); |
jalle1714 | 0:05dee1d3d857 | 6 | char path[1000] = ""; |
jalle1714 | 0:05dee1d3d857 | 7 | unsigned char path_length = 0; // the length of the path so far |
jalle1714 | 0:05dee1d3d857 | 8 | int sensors[5]; |
jalle1714 | 1:f4db9b0bb5e2 | 9 | DigitalIn pb(p21); |
jalle1714 | 0:05dee1d3d857 | 10 | |
jalle1714 | 0:05dee1d3d857 | 11 | // Minimum and maximum motor speeds |
jalle1714 | 0:05dee1d3d857 | 12 | #define MAX 0.25 |
jalle1714 | 0:05dee1d3d857 | 13 | #define MIN 0 |
jalle1714 | 0:05dee1d3d857 | 14 | |
jalle1714 | 0:05dee1d3d857 | 15 | // PID terms |
jalle1714 | 0:05dee1d3d857 | 16 | #define P_TERM 1 |
jalle1714 | 0:05dee1d3d857 | 17 | #define I_TERM 0 |
jalle1714 | 0:05dee1d3d857 | 18 | #define D_TERM 20 |
jalle1714 | 0:05dee1d3d857 | 19 | |
jalle1714 | 2:73f7cc4c30c7 | 20 | /*========================================================================================================================== |
jalle1714 | 0:05dee1d3d857 | 21 | FOLLOW LINE |
jalle1714 | 2:73f7cc4c30c7 | 22 | ============================================================================================================================*/ |
jalle1714 | 0:05dee1d3d857 | 23 | void follow_line() |
jalle1714 | 2:73f7cc4c30c7 | 24 | { |
jalle1714 | 0:05dee1d3d857 | 25 | |
jalle1714 | 0:05dee1d3d857 | 26 | m3pi.locate(0,1); |
jalle1714 | 0:05dee1d3d857 | 27 | m3pi.cls(); |
jalle1714 | 0:05dee1d3d857 | 28 | m3pi.printf("GO!!"); |
jalle1714 | 0:05dee1d3d857 | 29 | |
jalle1714 | 0:05dee1d3d857 | 30 | //wait(2.0); |
jalle1714 | 0:05dee1d3d857 | 31 | |
jalle1714 | 0:05dee1d3d857 | 32 | //m3pi.sensor_auto_calibrate();//don't want this here. |
jalle1714 | 0:05dee1d3d857 | 33 | |
jalle1714 | 0:05dee1d3d857 | 34 | float right; |
jalle1714 | 0:05dee1d3d857 | 35 | float left; |
jalle1714 | 0:05dee1d3d857 | 36 | float current_pos_of_line = 0.0; |
jalle1714 | 0:05dee1d3d857 | 37 | float previous_pos_of_line = 0.0; |
jalle1714 | 0:05dee1d3d857 | 38 | float derivative,proportional,integral = 0; |
jalle1714 | 0:05dee1d3d857 | 39 | float power; |
jalle1714 | 0:05dee1d3d857 | 40 | float speed = MAX; |
jalle1714 | 0:05dee1d3d857 | 41 | int foundjunction=0; |
jalle1714 | 0:05dee1d3d857 | 42 | int countdown=100; //make sure we don't stop for a junction too soon after starting |
jalle1714 | 2:73f7cc4c30c7 | 43 | while (foundjunction==0) { |
jalle1714 | 0:05dee1d3d857 | 44 | |
jalle1714 | 0:05dee1d3d857 | 45 | // Get the position of the line. |
jalle1714 | 2:73f7cc4c30c7 | 46 | current_pos_of_line = m3pi.line_position(); |
jalle1714 | 0:05dee1d3d857 | 47 | proportional = current_pos_of_line; |
jalle1714 | 2:73f7cc4c30c7 | 48 | |
jalle1714 | 0:05dee1d3d857 | 49 | // Compute the derivative |
jalle1714 | 0:05dee1d3d857 | 50 | derivative = current_pos_of_line - previous_pos_of_line; |
jalle1714 | 2:73f7cc4c30c7 | 51 | |
jalle1714 | 0:05dee1d3d857 | 52 | // Compute the integral |
jalle1714 | 2:73f7cc4c30c7 | 53 | // integral += proportional; |
jalle1714 | 2:73f7cc4c30c7 | 54 | |
jalle1714 | 0:05dee1d3d857 | 55 | // Remember the last position. |
jalle1714 | 0:05dee1d3d857 | 56 | previous_pos_of_line = current_pos_of_line; |
jalle1714 | 2:73f7cc4c30c7 | 57 | |
jalle1714 | 0:05dee1d3d857 | 58 | // Compute the power |
jalle1714 | 0:05dee1d3d857 | 59 | power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ; |
jalle1714 | 2:73f7cc4c30c7 | 60 | |
jalle1714 | 2:73f7cc4c30c7 | 61 | // Compute new speeds |
jalle1714 | 0:05dee1d3d857 | 62 | right = speed+power; |
jalle1714 | 0:05dee1d3d857 | 63 | left = speed-power; |
jalle1714 | 2:73f7cc4c30c7 | 64 | |
jalle1714 | 0:05dee1d3d857 | 65 | // limit checks |
jalle1714 | 0:05dee1d3d857 | 66 | if (right < MIN) |
jalle1714 | 0:05dee1d3d857 | 67 | right = MIN; |
jalle1714 | 0:05dee1d3d857 | 68 | else if (right > MAX) |
jalle1714 | 0:05dee1d3d857 | 69 | right = MAX; |
jalle1714 | 2:73f7cc4c30c7 | 70 | |
jalle1714 | 0:05dee1d3d857 | 71 | if (left < MIN) |
jalle1714 | 0:05dee1d3d857 | 72 | left = MIN; |
jalle1714 | 0:05dee1d3d857 | 73 | else if (left > MAX) |
jalle1714 | 0:05dee1d3d857 | 74 | left = MAX; |
jalle1714 | 2:73f7cc4c30c7 | 75 | |
jalle1714 | 2:73f7cc4c30c7 | 76 | // set speed |
jalle1714 | 0:05dee1d3d857 | 77 | m3pi.left_motor(left); |
jalle1714 | 0:05dee1d3d857 | 78 | m3pi.right_motor(right); |
jalle1714 | 2:73f7cc4c30c7 | 79 | |
jalle1714 | 2:73f7cc4c30c7 | 80 | if (countdown>0) countdown--; |
jalle1714 | 2:73f7cc4c30c7 | 81 | else { |
jalle1714 | 2:73f7cc4c30c7 | 82 | // Next, we are going to use the sensors to look for whether there is still a line ahead |
jalle1714 | 2:73f7cc4c30c7 | 83 | // and try to detect dead ends and possible left or right turns. |
jalle1714 | 2:73f7cc4c30c7 | 84 | m3pi.readsensor(sensors); |
jalle1714 | 2:73f7cc4c30c7 | 85 | |
jalle1714 | 2:73f7cc4c30c7 | 86 | if((sensors[1] < 400) && (sensors[2] < 400) && (sensors[3] < 400)) { |
jalle1714 | 2:73f7cc4c30c7 | 87 | // There is no line visible ahead, and we didn't see any |
jalle1714 | 2:73f7cc4c30c7 | 88 | // intersection. Must be a dead end. |
jalle1714 | 2:73f7cc4c30c7 | 89 | foundjunction=1; |
jalle1714 | 2:73f7cc4c30c7 | 90 | return; |
jalle1714 | 2:73f7cc4c30c7 | 91 | } else if((sensors[0] > 700) || (sensors[4] > 700)) { |
jalle1714 | 0:05dee1d3d857 | 92 | // Found an intersection. |
jalle1714 | 0:05dee1d3d857 | 93 | foundjunction=1; |
jalle1714 | 0:05dee1d3d857 | 94 | return; |
jalle1714 | 2:73f7cc4c30c7 | 95 | } else foundjunction=0; |
jalle1714 | 2:73f7cc4c30c7 | 96 | |
jalle1714 | 2:73f7cc4c30c7 | 97 | } |
jalle1714 | 2:73f7cc4c30c7 | 98 | } |
jalle1714 | 2:73f7cc4c30c7 | 99 | } |
jalle1714 | 2:73f7cc4c30c7 | 100 | /*========================================================================================================================= |
jalle1714 | 0:05dee1d3d857 | 101 | TURN |
jalle1714 | 2:73f7cc4c30c7 | 102 | ===========================================================================================================================*/ |
jalle1714 | 0:05dee1d3d857 | 103 | // This function decides which way to turn during the learning phase of |
jalle1714 | 0:05dee1d3d857 | 104 | // maze solving. It uses the variables found_left, found_straight, and |
jalle1714 | 0:05dee1d3d857 | 105 | // found_right, which indicate whether there is an exit in each of the |
jalle1714 | 0:05dee1d3d857 | 106 | // three directions, applying the "left hand on the wall" strategy. |
jalle1714 | 0:05dee1d3d857 | 107 | |
jalle1714 | 0:05dee1d3d857 | 108 | char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right) |
jalle1714 | 0:05dee1d3d857 | 109 | { |
jalle1714 | 0:05dee1d3d857 | 110 | // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm |
jalle1714 | 0:05dee1d3d857 | 111 | if(found_left) |
jalle1714 | 0:05dee1d3d857 | 112 | return 'L'; |
jalle1714 | 0:05dee1d3d857 | 113 | else if(found_forward) |
jalle1714 | 0:05dee1d3d857 | 114 | return 'F'; |
jalle1714 | 0:05dee1d3d857 | 115 | else if(found_right) |
jalle1714 | 0:05dee1d3d857 | 116 | return 'R'; |
jalle1714 | 0:05dee1d3d857 | 117 | else |
jalle1714 | 0:05dee1d3d857 | 118 | return 'B'; |
jalle1714 | 0:05dee1d3d857 | 119 | } |
jalle1714 | 2:73f7cc4c30c7 | 120 | /*========================================================================================================================== |
jalle1714 | 0:05dee1d3d857 | 121 | DO TURN |
jalle1714 | 2:73f7cc4c30c7 | 122 | ============================================================================================================================*/ |
jalle1714 | 0:05dee1d3d857 | 123 | |
jalle1714 | 0:05dee1d3d857 | 124 | void doturn(unsigned char dir) |
jalle1714 | 0:05dee1d3d857 | 125 | { |
jalle1714 | 2:73f7cc4c30c7 | 126 | if (dir=='L') { |
jalle1714 | 2:73f7cc4c30c7 | 127 | m3pi.left(0.25); |
jalle1714 | 2:73f7cc4c30c7 | 128 | wait(0.28); |
jalle1714 | 2:73f7cc4c30c7 | 129 | } else if(dir=='R') { |
jalle1714 | 2:73f7cc4c30c7 | 130 | m3pi.right(0.25); |
jalle1714 | 2:73f7cc4c30c7 | 131 | wait(0.28); |
jalle1714 | 2:73f7cc4c30c7 | 132 | } |
jalle1714 | 2:73f7cc4c30c7 | 133 | //else if(dir=='F') |
jalle1714 | 2:73f7cc4c30c7 | 134 | //{m3pi.forward(0.3);wait(0.15);} |
jalle1714 | 2:73f7cc4c30c7 | 135 | else if(dir=='B') { |
jalle1714 | 2:73f7cc4c30c7 | 136 | m3pi.right(0.25); |
jalle1714 | 2:73f7cc4c30c7 | 137 | wait(0.6); |
jalle1714 | 2:73f7cc4c30c7 | 138 | } |
jalle1714 | 2:73f7cc4c30c7 | 139 | |
jalle1714 | 2:73f7cc4c30c7 | 140 | m3pi.forward(0.1); |
jalle1714 | 2:73f7cc4c30c7 | 141 | wait(0.1); |
jalle1714 | 2:73f7cc4c30c7 | 142 | m3pi.forward(0); |
jalle1714 | 2:73f7cc4c30c7 | 143 | return; |
jalle1714 | 0:05dee1d3d857 | 144 | } |
jalle1714 | 2:73f7cc4c30c7 | 145 | /*========================================================================================================================== |
jalle1714 | 0:05dee1d3d857 | 146 | SIMPLIFY |
jalle1714 | 2:73f7cc4c30c7 | 147 | ============================================================================================================================*/ |
jalle1714 | 0:05dee1d3d857 | 148 | // change LBL to S (etc), to bypass dead ends |
jalle1714 | 0:05dee1d3d857 | 149 | void simplify() |
jalle1714 | 0:05dee1d3d857 | 150 | { |
jalle1714 | 0:05dee1d3d857 | 151 | // only simplify the path if the second-to-last turn was a 'B' |
jalle1714 | 0:05dee1d3d857 | 152 | if((path_length < 3) || (path[path_length-2] != 'B')) |
jalle1714 | 0:05dee1d3d857 | 153 | return; |
jalle1714 | 0:05dee1d3d857 | 154 | |
jalle1714 | 0:05dee1d3d857 | 155 | |
jalle1714 | 0:05dee1d3d857 | 156 | int total_angle = 0; |
jalle1714 | 0:05dee1d3d857 | 157 | int i; |
jalle1714 | 2:73f7cc4c30c7 | 158 | for(i=1; i<=3; i++) { |
jalle1714 | 2:73f7cc4c30c7 | 159 | switch(path[path_length-i]) { |
jalle1714 | 2:73f7cc4c30c7 | 160 | case 'R': |
jalle1714 | 2:73f7cc4c30c7 | 161 | total_angle += 90; |
jalle1714 | 2:73f7cc4c30c7 | 162 | break; |
jalle1714 | 2:73f7cc4c30c7 | 163 | case 'L': |
jalle1714 | 2:73f7cc4c30c7 | 164 | total_angle += 270; |
jalle1714 | 2:73f7cc4c30c7 | 165 | break; |
jalle1714 | 2:73f7cc4c30c7 | 166 | case 'B': |
jalle1714 | 2:73f7cc4c30c7 | 167 | total_angle += 180; |
jalle1714 | 2:73f7cc4c30c7 | 168 | break; |
jalle1714 | 0:05dee1d3d857 | 169 | } |
jalle1714 | 0:05dee1d3d857 | 170 | } |
jalle1714 | 0:05dee1d3d857 | 171 | |
jalle1714 | 0:05dee1d3d857 | 172 | // Get the angle as a number between 0 and 360 degrees. |
jalle1714 | 0:05dee1d3d857 | 173 | total_angle = total_angle % 360; |
jalle1714 | 0:05dee1d3d857 | 174 | |
jalle1714 | 0:05dee1d3d857 | 175 | // Replace all of those turns with a single one. |
jalle1714 | 2:73f7cc4c30c7 | 176 | switch(total_angle) { |
jalle1714 | 2:73f7cc4c30c7 | 177 | case 0: |
jalle1714 | 2:73f7cc4c30c7 | 178 | path[path_length - 3] = 'F'; |
jalle1714 | 2:73f7cc4c30c7 | 179 | break; |
jalle1714 | 2:73f7cc4c30c7 | 180 | case 90: |
jalle1714 | 2:73f7cc4c30c7 | 181 | path[path_length - 3] = 'R'; |
jalle1714 | 2:73f7cc4c30c7 | 182 | break; |
jalle1714 | 2:73f7cc4c30c7 | 183 | case 180: |
jalle1714 | 2:73f7cc4c30c7 | 184 | path[path_length - 3] = 'B'; |
jalle1714 | 2:73f7cc4c30c7 | 185 | break; |
jalle1714 | 2:73f7cc4c30c7 | 186 | case 270: |
jalle1714 | 2:73f7cc4c30c7 | 187 | path[path_length - 3] = 'L'; |
jalle1714 | 2:73f7cc4c30c7 | 188 | break; |
jalle1714 | 0:05dee1d3d857 | 189 | } |
jalle1714 | 0:05dee1d3d857 | 190 | |
jalle1714 | 0:05dee1d3d857 | 191 | // The path is now two steps shorter. |
jalle1714 | 0:05dee1d3d857 | 192 | path_length -= 2; |
jalle1714 | 0:05dee1d3d857 | 193 | } |
jalle1714 | 2:73f7cc4c30c7 | 194 | /*========================================================================================================================== |
jalle1714 | 1:f4db9b0bb5e2 | 195 | PRESSED |
jalle1714 | 2:73f7cc4c30c7 | 196 | ============================================================================================================================*/ |
jalle1714 | 1:f4db9b0bb5e2 | 197 | void pressed() |
jalle1714 | 1:f4db9b0bb5e2 | 198 | { |
jalle1714 | 2:73f7cc4c30c7 | 199 | |
jalle1714 | 2:73f7cc4c30c7 | 200 | // wait 1s to give time to GET OUT OF WAY |
jalle1714 | 2:73f7cc4c30c7 | 201 | wait(1); |
jalle1714 | 2:73f7cc4c30c7 | 202 | // |
jalle1714 | 1:f4db9b0bb5e2 | 203 | |
jalle1714 | 2:73f7cc4c30c7 | 204 | // Re-run the maze. It's not necessary to identify the |
jalle1714 | 2:73f7cc4c30c7 | 205 | // intersections, so this loop is really simple. |
jalle1714 | 2:73f7cc4c30c7 | 206 | int i; |
jalle1714 | 2:73f7cc4c30c7 | 207 | for(i=0; i<path_length; i++) { |
jalle1714 | 2:73f7cc4c30c7 | 208 | follow_line(); |
jalle1714 | 2:73f7cc4c30c7 | 209 | |
jalle1714 | 2:73f7cc4c30c7 | 210 | // Drive straight while slowing down |
jalle1714 | 2:73f7cc4c30c7 | 211 | //m3pi.forward(0.5); |
jalle1714 | 2:73f7cc4c30c7 | 212 | //wait(0.05); |
jalle1714 | 2:73f7cc4c30c7 | 213 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 214 | wait(0.2); |
jalle1714 | 1:f4db9b0bb5e2 | 215 | |
jalle1714 | 2:73f7cc4c30c7 | 216 | // Make a turn according to the instruction stored in |
jalle1714 | 2:73f7cc4c30c7 | 217 | // path[i]. |
jalle1714 | 2:73f7cc4c30c7 | 218 | doturn(path[i]); |
jalle1714 | 2:73f7cc4c30c7 | 219 | } |
jalle1714 | 1:f4db9b0bb5e2 | 220 | |
jalle1714 | 2:73f7cc4c30c7 | 221 | // Follow the last segment up to the finish. |
jalle1714 | 2:73f7cc4c30c7 | 222 | follow_line(); |
jalle1714 | 2:73f7cc4c30c7 | 223 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 224 | wait(0.6); |
jalle1714 | 1:f4db9b0bb5e2 | 225 | |
jalle1714 | 2:73f7cc4c30c7 | 226 | //celebrate with music |
jalle1714 | 2:73f7cc4c30c7 | 227 | char dixie[]={'V','1','5','O','5','G','1','6','E','1','6','C','8','R','2','4','C','8','R','2','4','C','1','6','D','1','6','E','1','6','F','1','6','G','8','R','2','4','G','8','R','2','4','G','8','E','1','6'}; |
jalle1714 | 2:73f7cc4c30c7 | 228 | int numb=49; |
jalle1714 | 2:73f7cc4c30c7 | 229 | m3pi.playtune(dixie,numb); |
jalle1714 | 2:73f7cc4c30c7 | 230 | return; |
jalle1714 | 2:73f7cc4c30c7 | 231 | |
jalle1714 | 2:73f7cc4c30c7 | 232 | // Now we should be at the finish! Restart the loop. |
jalle1714 | 2:73f7cc4c30c7 | 233 | |
jalle1714 | 1:f4db9b0bb5e2 | 234 | } |
jalle1714 | 0:05dee1d3d857 | 235 | |
jalle1714 | 2:73f7cc4c30c7 | 236 | /*========================================================================================================================= |
jalle1714 | 0:05dee1d3d857 | 237 | MAZESOLVE |
jalle1714 | 2:73f7cc4c30c7 | 238 | ===========================================================================================================================*/ |
jalle1714 | 0:05dee1d3d857 | 239 | // This function is called once, from main.c. |
jalle1714 | 0:05dee1d3d857 | 240 | void mazesolve() |
jalle1714 | 0:05dee1d3d857 | 241 | { |
jalle1714 | 1:f4db9b0bb5e2 | 242 | |
jalle1714 | 2:73f7cc4c30c7 | 243 | // These variables record whether the robot has seen a line to the |
jalle1714 | 0:05dee1d3d857 | 244 | // left, straight ahead, and right, while examining the current |
jalle1714 | 0:05dee1d3d857 | 245 | // intersection. |
jalle1714 | 2:73f7cc4c30c7 | 246 | unsigned char found_left=0; |
jalle1714 | 2:73f7cc4c30c7 | 247 | unsigned char found_forward=0; |
jalle1714 | 2:73f7cc4c30c7 | 248 | unsigned char found_right=0; |
jalle1714 | 2:73f7cc4c30c7 | 249 | unsigned char found_back=0; |
jalle1714 | 0:05dee1d3d857 | 250 | int sensors[5]; |
jalle1714 | 0:05dee1d3d857 | 251 | // Loop until we have solved the maze. |
jalle1714 | 2:73f7cc4c30c7 | 252 | while(1) { |
jalle1714 | 2:73f7cc4c30c7 | 253 | |
jalle1714 | 0:05dee1d3d857 | 254 | // Follow the line until an intersection is detected |
jalle1714 | 0:05dee1d3d857 | 255 | follow_line(); |
jalle1714 | 0:05dee1d3d857 | 256 | |
jalle1714 | 0:05dee1d3d857 | 257 | // Bump forward a bit in case sensor was triggered at an angle |
jalle1714 | 0:05dee1d3d857 | 258 | m3pi.forward(0.1); |
jalle1714 | 2:73f7cc4c30c7 | 259 | wait(0.1); |
jalle1714 | 2:73f7cc4c30c7 | 260 | |
jalle1714 | 2:73f7cc4c30c7 | 261 | found_left=0; |
jalle1714 | 2:73f7cc4c30c7 | 262 | found_forward=0; |
jalle1714 | 2:73f7cc4c30c7 | 263 | found_right=0; |
jalle1714 | 2:73f7cc4c30c7 | 264 | |
jalle1714 | 2:73f7cc4c30c7 | 265 | m3pi.readsensor(sensors);/*sensors are turned off outside the follow line |
jalle1714 | 2:73f7cc4c30c7 | 266 | function so we must call sensors eachtime they are needed*/ |
jalle1714 | 2:73f7cc4c30c7 | 267 | |
jalle1714 | 2:73f7cc4c30c7 | 268 | // Check for left and right exits. |
jalle1714 | 2:73f7cc4c30c7 | 269 | if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700)) { |
jalle1714 | 0:05dee1d3d857 | 270 | found_back=1; |
jalle1714 | 0:05dee1d3d857 | 271 | } |
jalle1714 | 2:73f7cc4c30c7 | 272 | |
jalle1714 | 2:73f7cc4c30c7 | 273 | if(sensors[0] > 700) { |
jalle1714 | 2:73f7cc4c30c7 | 274 | // Drive straight a bit more - this is enough to line up our |
jalle1714 | 2:73f7cc4c30c7 | 275 | // wheels with the intersection. |
jalle1714 | 2:73f7cc4c30c7 | 276 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 277 | wait(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 278 | m3pi.readsensor(sensors);//what's beyond the intersection |
jalle1714 | 2:73f7cc4c30c7 | 279 | // Check for the ending spot. |
jalle1714 | 2:73f7cc4c30c7 | 280 | // If all five sensors are on dark black, we have |
jalle1714 | 2:73f7cc4c30c7 | 281 | // solved the maze. |
jalle1714 | 2:73f7cc4c30c7 | 282 | if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) { |
jalle1714 | 0:05dee1d3d857 | 283 | //move upon the home pad to show off |
jalle1714 | 2:73f7cc4c30c7 | 284 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 285 | wait(0.4); |
jalle1714 | 2:73f7cc4c30c7 | 286 | m3pi.printf("Press P21 Restart"); |
jalle1714 | 2:73f7cc4c30c7 | 287 | break; |
jalle1714 | 2:73f7cc4c30c7 | 288 | } else found_left = 1; |
jalle1714 | 2:73f7cc4c30c7 | 289 | |
jalle1714 | 2:73f7cc4c30c7 | 290 | } |
jalle1714 | 2:73f7cc4c30c7 | 291 | |
jalle1714 | 2:73f7cc4c30c7 | 292 | else if(sensors[4] > 700 ) { |
jalle1714 | 0:05dee1d3d857 | 293 | //move wheels to intersection |
jalle1714 | 2:73f7cc4c30c7 | 294 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 295 | wait(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 296 | //what is past the intersection |
jalle1714 | 2:73f7cc4c30c7 | 297 | m3pi.readsensor(sensors); |
jalle1714 | 2:73f7cc4c30c7 | 298 | // Check for the ending spot. |
jalle1714 | 2:73f7cc4c30c7 | 299 | // If all five sensors are on dark black, we have |
jalle1714 | 2:73f7cc4c30c7 | 300 | // solved the maze. |
jalle1714 | 2:73f7cc4c30c7 | 301 | if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900)) { |
jalle1714 | 2:73f7cc4c30c7 | 302 | m3pi.forward(0.2); |
jalle1714 | 2:73f7cc4c30c7 | 303 | wait(0.4); |
jalle1714 | 2:73f7cc4c30c7 | 304 | m3pi.printf("Press P21 Restart"); |
jalle1714 | 2:73f7cc4c30c7 | 305 | break; |
jalle1714 | 2:73f7cc4c30c7 | 306 | } |
jalle1714 | 2:73f7cc4c30c7 | 307 | //can we go forward |
jalle1714 | 2:73f7cc4c30c7 | 308 | else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700)) { |
jalle1714 | 0:05dee1d3d857 | 309 | found_forward = 1; |
jalle1714 | 2:73f7cc4c30c7 | 310 | |
jalle1714 | 2:73f7cc4c30c7 | 311 | } |
jalle1714 | 2:73f7cc4c30c7 | 312 | //then go right |
jalle1714 | 2:73f7cc4c30c7 | 313 | else found_right=1; |
jalle1714 | 2:73f7cc4c30c7 | 314 | } |
jalle1714 | 2:73f7cc4c30c7 | 315 | |
jalle1714 | 2:73f7cc4c30c7 | 316 | //debug code |
jalle1714 | 0:05dee1d3d857 | 317 | m3pi.cls(); |
jalle1714 | 0:05dee1d3d857 | 318 | if (found_left==1) |
jalle1714 | 2:73f7cc4c30c7 | 319 | m3pi.printf("L"); |
jalle1714 | 0:05dee1d3d857 | 320 | if (found_right==1) |
jalle1714 | 2:73f7cc4c30c7 | 321 | m3pi.printf("R"); |
jalle1714 | 0:05dee1d3d857 | 322 | if (found_forward==1) |
jalle1714 | 2:73f7cc4c30c7 | 323 | m3pi.printf("F"); |
jalle1714 | 0:05dee1d3d857 | 324 | if (found_back==1) |
jalle1714 | 2:73f7cc4c30c7 | 325 | m3pi.printf("B"); |
jalle1714 | 0:05dee1d3d857 | 326 | //wait (3); |
jalle1714 | 2:73f7cc4c30c7 | 327 | |
jalle1714 | 0:05dee1d3d857 | 328 | |
jalle1714 | 0:05dee1d3d857 | 329 | unsigned char dir = turn(found_left, found_forward, found_right); |
jalle1714 | 0:05dee1d3d857 | 330 | |
jalle1714 | 0:05dee1d3d857 | 331 | // Make the turn indicated by the path. |
jalle1714 | 0:05dee1d3d857 | 332 | //doturn(dir); |
jalle1714 | 0:05dee1d3d857 | 333 | doturn(dir); |
jalle1714 | 0:05dee1d3d857 | 334 | // Store the intersection in the path variable. |
jalle1714 | 0:05dee1d3d857 | 335 | path[path_length] = dir; |
jalle1714 | 0:05dee1d3d857 | 336 | path_length ++; |
jalle1714 | 0:05dee1d3d857 | 337 | |
jalle1714 | 0:05dee1d3d857 | 338 | // Need to insert check to make sure that the path_length does not |
jalle1714 | 2:73f7cc4c30c7 | 339 | // exceed the bounds of the array. |
jalle1714 | 0:05dee1d3d857 | 340 | |
jalle1714 | 0:05dee1d3d857 | 341 | // Simplify the learned path. |
jalle1714 | 0:05dee1d3d857 | 342 | simplify(); |
jalle1714 | 2:73f7cc4c30c7 | 343 | |
jalle1714 | 0:05dee1d3d857 | 344 | } |
jalle1714 | 2:73f7cc4c30c7 | 345 | |
jalle1714 | 2:73f7cc4c30c7 | 346 | // Solved the maze! |
jalle1714 | 0:05dee1d3d857 | 347 | |
jalle1714 | 0:05dee1d3d857 | 348 | // Now enter an infinite loop - we can re-run the maze as many |
jalle1714 | 0:05dee1d3d857 | 349 | // times as we want to. |
jalle1714 | 2:73f7cc4c30c7 | 350 | while(1) { |
jalle1714 | 2:73f7cc4c30c7 | 351 | m3pi.forward(0.0); |
jalle1714 | 2:73f7cc4c30c7 | 352 | pb.mode(PullUp); |
jalle1714 | 2:73f7cc4c30c7 | 353 | wait(1); |
jalle1714 | 2:73f7cc4c30c7 | 354 | |
jalle1714 | 2:73f7cc4c30c7 | 355 | if(pb) { |
jalle1714 | 2:73f7cc4c30c7 | 356 | do { |
jalle1714 | 2:73f7cc4c30c7 | 357 | |
jalle1714 | 2:73f7cc4c30c7 | 358 | } while(pb); |
jalle1714 | 1:f4db9b0bb5e2 | 359 | pressed(); |
jalle1714 | 2:73f7cc4c30c7 | 360 | } |
jalle1714 | 2:73f7cc4c30c7 | 361 | |
jalle1714 | 1:f4db9b0bb5e2 | 362 | } |
jalle1714 | 2:73f7cc4c30c7 | 363 | |
jalle1714 | 0:05dee1d3d857 | 364 | |
jalle1714 | 0:05dee1d3d857 | 365 | } |
jalle1714 | 0:05dee1d3d857 | 366 | |
jalle1714 | 2:73f7cc4c30c7 | 367 | /*========================================================================================================================= |
jalle1714 | 0:05dee1d3d857 | 368 | MAIN |
jalle1714 | 2:73f7cc4c30c7 | 369 | ===========================================================================================================================*/ |
jalle1714 | 2:73f7cc4c30c7 | 370 | int main() |
jalle1714 | 2:73f7cc4c30c7 | 371 | { |
jalle1714 | 2:73f7cc4c30c7 | 372 | // int sensors[5]; |
jalle1714 | 0:05dee1d3d857 | 373 | m3pi.locate(0,1); |
jalle1714 | 0:05dee1d3d857 | 374 | m3pi.sensor_auto_calibrate(); |
jalle1714 | 0:05dee1d3d857 | 375 | m3pi.printf("MazeSolve"); |
jalle1714 | 0:05dee1d3d857 | 376 | |
jalle1714 | 0:05dee1d3d857 | 377 | wait(2.0); |
jalle1714 | 0:05dee1d3d857 | 378 | |
jalle1714 | 2:73f7cc4c30c7 | 379 | mazesolve(); |
jalle1714 | 0:05dee1d3d857 | 380 | |
jalle1714 | 2:73f7cc4c30c7 | 381 | m3pi.forward(0.0); |
jalle1714 | 0:05dee1d3d857 | 382 | } |