James Allen / Mbed 2 deprecated MyMazeSolver

working modified m3pimazesolver

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }