Jon Marsh / Mbed 2 deprecated m3pi_MazeSolver

This is work in progress - maze solving with m3pi robot. Not complete

Dependencies:   mbed m3pimaze

main.cpp@1:3ac7462953df, 2011-03-03 (annotated)

Committer:
jonmarsh
Date:
Thu Mar 03 10:18:49 2011 +0000
Revision:
1:3ac7462953df
Parent:
0:6e52eae6412a 
With renamed m3pimaze library

Who changed what in which revision?

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