James Allen / Mbed 2 deprecated MyMazeSolver

working modified m3pimazesolver

Dependencies:   mbed

main.cpp@0:05dee1d3d857, 2013-04-04 (annotated)

Committer:
jalle1714
Date:
Thu Apr 04 03:44:59 2013 +0000
Revision:
0:05dee1d3d857
Child:
1:f4db9b0bb5e2
m3pimazesolver modified using left hand rule.

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