James Allen / Mbed 2 deprecated MyMazeSolver

working modified m3pimazesolver

Dependencies:   mbed

main.cpp@1:f4db9b0bb5e2, 2013-04-05 (annotated)

Committer:
jalle1714
Date:
Fri Apr 05 18:46:50 2013 +0000
Revision:
1:f4db9b0bb5e2
Parent:
0:05dee1d3d857
Child:
2:73f7cc4c30c7
Added Push Button P21

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 0:05dee1d3d857 20 /*=================================================================
jalle1714 0:05dee1d3d857 21 FOLLOW LINE
jalle1714 0:05dee1d3d857 22 ===================================================================*/
jalle1714 0:05dee1d3d857 23 void follow_line()
jalle1714 0:05dee1d3d857 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 0:05dee1d3d857 43 while (foundjunction==0)
jalle1714 0:05dee1d3d857 44 {
jalle1714 0:05dee1d3d857 45
jalle1714 0:05dee1d3d857 46 // Get the position of the line.
jalle1714 0:05dee1d3d857 47 current_pos_of_line = m3pi.line_position();
jalle1714 0:05dee1d3d857 48 proportional = current_pos_of_line;
jalle1714 0:05dee1d3d857 49
jalle1714 0:05dee1d3d857 50 // Compute the derivative
jalle1714 0:05dee1d3d857 51 derivative = current_pos_of_line - previous_pos_of_line;
jalle1714 0:05dee1d3d857 52
jalle1714 0:05dee1d3d857 53 // Compute the integral
jalle1714 0:05dee1d3d857 54 // integral += proportional;
jalle1714 0:05dee1d3d857 55
jalle1714 0:05dee1d3d857 56 // Remember the last position.
jalle1714 0:05dee1d3d857 57 previous_pos_of_line = current_pos_of_line;
jalle1714 0:05dee1d3d857 58
jalle1714 0:05dee1d3d857 59 // Compute the power
jalle1714 0:05dee1d3d857 60 power = (proportional * (P_TERM) ) + (integral*(I_TERM)) + (derivative*(D_TERM)) ;
jalle1714 0:05dee1d3d857 61
jalle1714 0:05dee1d3d857 62 // Compute new speeds
jalle1714 0:05dee1d3d857 63 right = speed+power;
jalle1714 0:05dee1d3d857 64 left = speed-power;
jalle1714 0:05dee1d3d857 65
jalle1714 0:05dee1d3d857 66 // limit checks
jalle1714 0:05dee1d3d857 67 if (right < MIN)
jalle1714 0:05dee1d3d857 68 right = MIN;
jalle1714 0:05dee1d3d857 69 else if (right > MAX)
jalle1714 0:05dee1d3d857 70 right = MAX;
jalle1714 0:05dee1d3d857 71
jalle1714 0:05dee1d3d857 72 if (left < MIN)
jalle1714 0:05dee1d3d857 73 left = MIN;
jalle1714 0:05dee1d3d857 74 else if (left > MAX)
jalle1714 0:05dee1d3d857 75 left = MAX;
jalle1714 0:05dee1d3d857 76
jalle1714 0:05dee1d3d857 77 // set speed
jalle1714 0:05dee1d3d857 78 m3pi.left_motor(left);
jalle1714 0:05dee1d3d857 79 m3pi.right_motor(right);
jalle1714 0:05dee1d3d857 80
jalle1714 0:05dee1d3d857 81 if (countdown>0) countdown--; else {
jalle1714 0:05dee1d3d857 82 // Next, we are going to use the sensors to look for whether there is still a line ahead
jalle1714 0:05dee1d3d857 83 // and try to detect dead ends and possible left or right turns.
jalle1714 0:05dee1d3d857 84 m3pi.readsensor(sensors);
jalle1714 0:05dee1d3d857 85
jalle1714 0:05dee1d3d857 86 if((sensors[1] < 400) && (sensors[2] < 400) && (sensors[3] < 400))
jalle1714 0:05dee1d3d857 87 {
jalle1714 0:05dee1d3d857 88 // There is no line visible ahead, and we didn't see any
jalle1714 0:05dee1d3d857 89 // intersection. Must be a dead end.
jalle1714 0:05dee1d3d857 90 foundjunction=1;
jalle1714 0:05dee1d3d857 91 return;
jalle1714 0:05dee1d3d857 92 }
jalle1714 0:05dee1d3d857 93 else if((sensors[0] > 700) || (sensors[4] > 700))
jalle1714 0:05dee1d3d857 94 {
jalle1714 0:05dee1d3d857 95 // Found an intersection.
jalle1714 0:05dee1d3d857 96 foundjunction=1;
jalle1714 0:05dee1d3d857 97 return;
jalle1714 0:05dee1d3d857 98 }
jalle1714 0:05dee1d3d857 99 else foundjunction=0;
jalle1714 0:05dee1d3d857 100
jalle1714 0:05dee1d3d857 101 }}}
jalle1714 0:05dee1d3d857 102 /*===========================================================================================================
jalle1714 0:05dee1d3d857 103 TURN
jalle1714 0:05dee1d3d857 104 =============================================================================================================*/
jalle1714 0:05dee1d3d857 105 // This function decides which way to turn during the learning phase of
jalle1714 0:05dee1d3d857 106 // maze solving. It uses the variables found_left, found_straight, and
jalle1714 0:05dee1d3d857 107 // found_right, which indicate whether there is an exit in each of the
jalle1714 0:05dee1d3d857 108 // three directions, applying the "left hand on the wall" strategy.
jalle1714 0:05dee1d3d857 109
jalle1714 0:05dee1d3d857 110 char turn(unsigned char found_left, unsigned char found_forward, unsigned char found_right)
jalle1714 0:05dee1d3d857 111 {
jalle1714 0:05dee1d3d857 112 // The order of the statements in this "if" is sufficient to implement a follow left-hand wall algorithm
jalle1714 0:05dee1d3d857 113 if(found_left)
jalle1714 0:05dee1d3d857 114 return 'L';
jalle1714 0:05dee1d3d857 115 else if(found_forward)
jalle1714 0:05dee1d3d857 116 return 'F';
jalle1714 0:05dee1d3d857 117 else if(found_right)
jalle1714 0:05dee1d3d857 118 return 'R';
jalle1714 0:05dee1d3d857 119 else
jalle1714 0:05dee1d3d857 120 return 'B';
jalle1714 0:05dee1d3d857 121 }
jalle1714 0:05dee1d3d857 122 /*============================================================================================================
jalle1714 0:05dee1d3d857 123 DO TURN
jalle1714 0:05dee1d3d857 124 ==============================================================================================================*/
jalle1714 0:05dee1d3d857 125
jalle1714 0:05dee1d3d857 126 void doturn(unsigned char dir)
jalle1714 0:05dee1d3d857 127 {
jalle1714 0:05dee1d3d857 128 if (dir=='L')
jalle1714 0:05dee1d3d857 129 {m3pi.left(0.25);wait(0.28);}
jalle1714 0:05dee1d3d857 130 else if(dir=='R')
jalle1714 0:05dee1d3d857 131 {m3pi.right(0.25);wait(0.28);}
jalle1714 0:05dee1d3d857 132 //else if(dir=='F')
jalle1714 0:05dee1d3d857 133 //{m3pi.forward(0.3);wait(0.15);}
jalle1714 0:05dee1d3d857 134 else if(dir=='B')
jalle1714 0:05dee1d3d857 135 {m3pi.right(0.25);wait(0.6);}
jalle1714 0:05dee1d3d857 136
jalle1714 0:05dee1d3d857 137 m3pi.forward(0.1);wait(0.1);m3pi.forward(0);
jalle1714 0:05dee1d3d857 138 return;
jalle1714 0:05dee1d3d857 139 }
jalle1714 0:05dee1d3d857 140 /*=============================================================================================
jalle1714 0:05dee1d3d857 141 SIMPLIFY
jalle1714 0:05dee1d3d857 142 ===============================================================================================*/
jalle1714 0:05dee1d3d857 143 // change LBL to S (etc), to bypass dead ends
jalle1714 0:05dee1d3d857 144 void simplify()
jalle1714 0:05dee1d3d857 145 {
jalle1714 0:05dee1d3d857 146 // only simplify the path if the second-to-last turn was a 'B'
jalle1714 0:05dee1d3d857 147 if((path_length < 3) || (path[path_length-2] != 'B'))
jalle1714 0:05dee1d3d857 148 return;
jalle1714 0:05dee1d3d857 149
jalle1714 0:05dee1d3d857 150
jalle1714 0:05dee1d3d857 151 int total_angle = 0;
jalle1714 0:05dee1d3d857 152 int i;
jalle1714 0:05dee1d3d857 153 for(i=1;i<=3;i++)
jalle1714 0:05dee1d3d857 154 {
jalle1714 0:05dee1d3d857 155 switch(path[path_length-i])
jalle1714 0:05dee1d3d857 156 {
jalle1714 0:05dee1d3d857 157 case 'R':
jalle1714 0:05dee1d3d857 158 total_angle += 90;
jalle1714 0:05dee1d3d857 159 break;
jalle1714 0:05dee1d3d857 160 case 'L':
jalle1714 0:05dee1d3d857 161 total_angle += 270;
jalle1714 0:05dee1d3d857 162 break;
jalle1714 0:05dee1d3d857 163 case 'B':
jalle1714 0:05dee1d3d857 164 total_angle += 180;
jalle1714 0:05dee1d3d857 165 break;
jalle1714 0:05dee1d3d857 166 }
jalle1714 0:05dee1d3d857 167 }
jalle1714 0:05dee1d3d857 168
jalle1714 0:05dee1d3d857 169 // Get the angle as a number between 0 and 360 degrees.
jalle1714 0:05dee1d3d857 170 total_angle = total_angle % 360;
jalle1714 0:05dee1d3d857 171
jalle1714 0:05dee1d3d857 172 // Replace all of those turns with a single one.
jalle1714 0:05dee1d3d857 173 switch(total_angle)
jalle1714 0:05dee1d3d857 174 {
jalle1714 0:05dee1d3d857 175 case 0:
jalle1714 0:05dee1d3d857 176 path[path_length - 3] = 'F';
jalle1714 0:05dee1d3d857 177 break;
jalle1714 0:05dee1d3d857 178 case 90:
jalle1714 0:05dee1d3d857 179 path[path_length - 3] = 'R';
jalle1714 0:05dee1d3d857 180 break;
jalle1714 0:05dee1d3d857 181 case 180:
jalle1714 0:05dee1d3d857 182 path[path_length - 3] = 'B';
jalle1714 0:05dee1d3d857 183 break;
jalle1714 0:05dee1d3d857 184 case 270:
jalle1714 0:05dee1d3d857 185 path[path_length - 3] = 'L';
jalle1714 0:05dee1d3d857 186 break;
jalle1714 0:05dee1d3d857 187 }
jalle1714 0:05dee1d3d857 188
jalle1714 0:05dee1d3d857 189 // The path is now two steps shorter.
jalle1714 0:05dee1d3d857 190 path_length -= 2;
jalle1714 0:05dee1d3d857 191 }
jalle1714 1:f4db9b0bb5e2 192 /*==============================================================================================================
jalle1714 1:f4db9b0bb5e2 193 PRESSED
jalle1714 1:f4db9b0bb5e2 194 ================================================================================================================*/
jalle1714 1:f4db9b0bb5e2 195 void pressed()
jalle1714 1:f4db9b0bb5e2 196 {
jalle1714 1:f4db9b0bb5e2 197
jalle1714 1:f4db9b0bb5e2 198 // wait 15s to give time to turn off, or put the robot back to the start
jalle1714 1:f4db9b0bb5e2 199 wait(2);
jalle1714 1:f4db9b0bb5e2 200 // ideally we would use a button press here
jalle1714 1:f4db9b0bb5e2 201 // but I don't think it can easily be read
jalle1714 1:f4db9b0bb5e2 202
jalle1714 1:f4db9b0bb5e2 203 // Re-run the maze. It's not necessary to identify the
jalle1714 1:f4db9b0bb5e2 204 // intersections, so this loop is really simple.
jalle1714 1:f4db9b0bb5e2 205 int i;
jalle1714 1:f4db9b0bb5e2 206 for(i=0;i<path_length;i++)
jalle1714 1:f4db9b0bb5e2 207 {
jalle1714 1:f4db9b0bb5e2 208 follow_line();
jalle1714 1:f4db9b0bb5e2 209
jalle1714 1:f4db9b0bb5e2 210 // Drive straight while slowing down
jalle1714 1:f4db9b0bb5e2 211 //m3pi.forward(0.5);
jalle1714 1:f4db9b0bb5e2 212 //wait(0.05);
jalle1714 1:f4db9b0bb5e2 213 m3pi.forward(0.2);
jalle1714 1:f4db9b0bb5e2 214 wait(0.2);
jalle1714 1:f4db9b0bb5e2 215
jalle1714 1:f4db9b0bb5e2 216 // Make a turn according to the instruction stored in
jalle1714 1:f4db9b0bb5e2 217 // path[i].
jalle1714 1:f4db9b0bb5e2 218 doturn(path[i]);
jalle1714 1:f4db9b0bb5e2 219 }
jalle1714 1:f4db9b0bb5e2 220
jalle1714 1:f4db9b0bb5e2 221 // Follow the last segment up to the finish.
jalle1714 1:f4db9b0bb5e2 222 follow_line();
jalle1714 1:f4db9b0bb5e2 223 m3pi.forward(0.2);
jalle1714 1:f4db9b0bb5e2 224 wait(0.6);
jalle1714 1:f4db9b0bb5e2 225 return;
jalle1714 1:f4db9b0bb5e2 226
jalle1714 1:f4db9b0bb5e2 227 // Now we should be at the finish! Restart the loop.
jalle1714 1:f4db9b0bb5e2 228
jalle1714 1:f4db9b0bb5e2 229 }
jalle1714 0:05dee1d3d857 230
jalle1714 0:05dee1d3d857 231 /*========================================================================================================
jalle1714 0:05dee1d3d857 232 MAZESOLVE
jalle1714 0:05dee1d3d857 233 ==========================================================================================================*/
jalle1714 0:05dee1d3d857 234 // This function is called once, from main.c.
jalle1714 0:05dee1d3d857 235 void mazesolve()
jalle1714 0:05dee1d3d857 236 {
jalle1714 1:f4db9b0bb5e2 237
jalle1714 0:05dee1d3d857 238 // These variables record whether the robot has seen a line to the
jalle1714 0:05dee1d3d857 239 // left, straight ahead, and right, while examining the current
jalle1714 0:05dee1d3d857 240 // intersection.
jalle1714 0:05dee1d3d857 241 unsigned char found_left=0;
jalle1714 0:05dee1d3d857 242 unsigned char found_forward=0;
jalle1714 0:05dee1d3d857 243 unsigned char found_right=0;
jalle1714 0:05dee1d3d857 244 unsigned char found_back=0;
jalle1714 0:05dee1d3d857 245 int sensors[5];
jalle1714 0:05dee1d3d857 246 // Loop until we have solved the maze.
jalle1714 0:05dee1d3d857 247 while(1)
jalle1714 0:05dee1d3d857 248 {
jalle1714 0:05dee1d3d857 249
jalle1714 0:05dee1d3d857 250 // Follow the line until an intersection is detected
jalle1714 0:05dee1d3d857 251 follow_line();
jalle1714 0:05dee1d3d857 252
jalle1714 0:05dee1d3d857 253 // Bump forward a bit in case sensor was triggered at an angle
jalle1714 0:05dee1d3d857 254 m3pi.forward(0.1);
jalle1714 0:05dee1d3d857 255 wait(0.1);
jalle1714 0:05dee1d3d857 256
jalle1714 0:05dee1d3d857 257 found_left=0;found_forward=0;found_right=0;
jalle1714 0:05dee1d3d857 258
jalle1714 0:05dee1d3d857 259 m3pi.readsensor(sensors);//sensors are turned off outside the follow line function so we must call sensors eachtime they are needed
jalle1714 0:05dee1d3d857 260
jalle1714 0:05dee1d3d857 261 // Check for left and right exits.
jalle1714 0:05dee1d3d857 262 if((sensors[1]<700) && (sensors [2]<700) && (sensors[3]<700))
jalle1714 0:05dee1d3d857 263 {
jalle1714 0:05dee1d3d857 264 found_back=1;
jalle1714 0:05dee1d3d857 265 }
jalle1714 0:05dee1d3d857 266
jalle1714 0:05dee1d3d857 267 if(sensors[0] > 700)
jalle1714 0:05dee1d3d857 268 {
jalle1714 0:05dee1d3d857 269 // Drive straight a bit more - this is enough to line up our
jalle1714 0:05dee1d3d857 270 // wheels with the intersection.
jalle1714 0:05dee1d3d857 271 m3pi.forward(0.2);
jalle1714 0:05dee1d3d857 272 wait(0.2);
jalle1714 0:05dee1d3d857 273 m3pi.readsensor(sensors);//what's beyond the intersection
jalle1714 0:05dee1d3d857 274 // Check for the ending spot.
jalle1714 0:05dee1d3d857 275 // If all five sensors are on dark black, we have
jalle1714 0:05dee1d3d857 276 // solved the maze.
jalle1714 0:05dee1d3d857 277 if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
jalle1714 0:05dee1d3d857 278 {
jalle1714 0:05dee1d3d857 279 //move upon the home pad to show off
jalle1714 0:05dee1d3d857 280 m3pi.forward(0.2);
jalle1714 0:05dee1d3d857 281 wait(0.4);
jalle1714 1:f4db9b0bb5e2 282 m3pi.printf("Finished");
jalle1714 0:05dee1d3d857 283 break;
jalle1714 0:05dee1d3d857 284 }
jalle1714 0:05dee1d3d857 285 else found_left = 1;
jalle1714 0:05dee1d3d857 286
jalle1714 0:05dee1d3d857 287 }
jalle1714 0:05dee1d3d857 288
jalle1714 0:05dee1d3d857 289 else if(sensors[4] > 700 )
jalle1714 0:05dee1d3d857 290 {
jalle1714 0:05dee1d3d857 291 //move wheels to intersection
jalle1714 0:05dee1d3d857 292 m3pi.forward(0.2);
jalle1714 0:05dee1d3d857 293 wait(0.2);
jalle1714 0:05dee1d3d857 294 //what is past the intersection
jalle1714 0:05dee1d3d857 295 m3pi.readsensor(sensors);
jalle1714 0:05dee1d3d857 296 // Check for the ending spot.
jalle1714 0:05dee1d3d857 297 // If all five sensors are on dark black, we have
jalle1714 0:05dee1d3d857 298 // solved the maze.
jalle1714 0:05dee1d3d857 299 if((sensors[0]>900) && (sensors[1] > 900) && (sensors[2] > 900) && (sensors[3] > 900) && (sensors[4]>900))
jalle1714 0:05dee1d3d857 300 {
jalle1714 0:05dee1d3d857 301 m3pi.forward(0.2);
jalle1714 0:05dee1d3d857 302 wait(0.4);
jalle1714 1:f4db9b0bb5e2 303 m3pi.printf("Finished");
jalle1714 0:05dee1d3d857 304 break;
jalle1714 0:05dee1d3d857 305 }
jalle1714 0:05dee1d3d857 306 //can we go forward
jalle1714 0:05dee1d3d857 307 else if((sensors[1] > 700 )|| (sensors[2] > 700) || (sensors[3] > 700))
jalle1714 0:05dee1d3d857 308 {
jalle1714 0:05dee1d3d857 309 found_forward = 1;
jalle1714 0:05dee1d3d857 310
jalle1714 0:05dee1d3d857 311 }
jalle1714 0:05dee1d3d857 312 //then go right
jalle1714 0:05dee1d3d857 313 else found_right=1;
jalle1714 0:05dee1d3d857 314 }
jalle1714 0:05dee1d3d857 315
jalle1714 0:05dee1d3d857 316 //debug code
jalle1714 0:05dee1d3d857 317 m3pi.cls();
jalle1714 0:05dee1d3d857 318 if (found_left==1)
jalle1714 0:05dee1d3d857 319 m3pi.printf("L");
jalle1714 0:05dee1d3d857 320 if (found_right==1)
jalle1714 0:05dee1d3d857 321 m3pi.printf("R");
jalle1714 0:05dee1d3d857 322 if (found_forward==1)
jalle1714 0:05dee1d3d857 323 m3pi.printf("F");
jalle1714 0:05dee1d3d857 324 if (found_back==1)
jalle1714 0:05dee1d3d857 325 m3pi.printf("B");
jalle1714 0:05dee1d3d857 326 //wait (3);
jalle1714 0:05dee1d3d857 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 0:05dee1d3d857 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 0:05dee1d3d857 343
jalle1714 0:05dee1d3d857 344 }
jalle1714 0:05dee1d3d857 345
jalle1714 0:05dee1d3d857 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 0:05dee1d3d857 350 while(1)
jalle1714 0:05dee1d3d857 351 {
jalle1714 1:f4db9b0bb5e2 352 m3pi.forward(0.0);
jalle1714 1:f4db9b0bb5e2 353 pb.mode(PullUp);
jalle1714 1:f4db9b0bb5e2 354
jalle1714 1:f4db9b0bb5e2 355 if(pb)
jalle1714 0:05dee1d3d857 356 {
jalle1714 1:f4db9b0bb5e2 357 do
jalle1714 1:f4db9b0bb5e2 358 {
jalle1714 1:f4db9b0bb5e2 359
jalle1714 1:f4db9b0bb5e2 360 }
jalle1714 1:f4db9b0bb5e2 361 while(pb);
jalle1714 1:f4db9b0bb5e2 362 pressed();
jalle1714 1:f4db9b0bb5e2 363 }
jalle1714 0:05dee1d3d857 364
jalle1714 1:f4db9b0bb5e2 365 }
jalle1714 0:05dee1d3d857 366
jalle1714 0:05dee1d3d857 367
jalle1714 0:05dee1d3d857 368 }
jalle1714 0:05dee1d3d857 369
jalle1714 0:05dee1d3d857 370 /*========================================================================================================
jalle1714 0:05dee1d3d857 371 MAIN
jalle1714 0:05dee1d3d857 372 =========================================================================================================*/
jalle1714 0:05dee1d3d857 373 int main() {
jalle1714 0:05dee1d3d857 374 // int sensors[5];
jalle1714 0:05dee1d3d857 375 m3pi.locate(0,1);
jalle1714 0:05dee1d3d857 376 m3pi.sensor_auto_calibrate();
jalle1714 0:05dee1d3d857 377 m3pi.printf("MazeSolve");
jalle1714 0:05dee1d3d857 378
jalle1714 0:05dee1d3d857 379 wait(2.0);
jalle1714 0:05dee1d3d857 380
jalle1714 0:05dee1d3d857 381 mazesolve();
jalle1714 0:05dee1d3d857 382
jalle1714 0:05dee1d3d857 383 m3pi.forward(0.0);
jalle1714 0:05dee1d3d857 384 }