valerio rizzo / Mbed 2 deprecated m3pi_object-avoider

Line follower and object avoider

Dependencies:   mbed SHARPIR

main.cpp@0:df7d60d7a6ee, 2012-04-12 (annotated)

Committer:
nepol77
Date:
Thu Apr 12 14:37:54 2012 +0000
Revision:
0:df7d60d7a6ee
Hi guys, i will put the pictures on very soon!! Thanks for looking

Who changed what in which revision?

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