root / trunk / code / behaviors / formation_control / circle / circle.c @ 1725
History | View | Annotate | Download (13.5 KB)
| 1 | #include <dragonfly_lib.h> |
|---|---|
| 2 | #include <wl_basic.h> |
| 3 | #include <encoders.h> |
| 4 | #include "circle.h" |
| 5 | |
| 6 | /*
|
| 7 | Current situation: |
| 8 | go to the center. ends when each edge robto send "I am here" message |
| 9 | |
| 10 | TODO: |
| 11 | Center: |
| 12 | 1. Make a move forward method/state to make the circle begin moving as a group. |
| 13 | |
| 14 | Edge: |
| 15 | 2. Find a way to follow the middle robot and preserve circle structure, or follow a directional command given by the center robot. |
| 16 | 3. Fix the error case with wireless - if the center robot does not receive the first exist packet, the robot keeps sending it and will cause error later. |
| 17 | |
| 18 | In general: |
| 19 | More testing. Especially with robots that are actually working. |
| 20 | */ |
| 21 | |
| 22 | |
| 23 | /*
|
| 24 | This program is used to make robots target a center (leader) robot using the BOM, |
| 25 | then drive toward it and stop at a certain distance away. |
| 26 | |
| 27 | The distance will eventually be adjustable. |
| 28 | |
| 29 | With adjustment, the leader robot will be able to turn and use its standardized |
| 30 | rangefinder to reposition or space the robots evenly. |
| 31 | |
| 32 | AuTHORS: Nico, Alex, Reva, Echo, Steve, Joel |
| 33 | */ |
| 34 | |
| 35 | |
| 36 | /*
|
| 37 | TODO: |
| 38 | Used: Bots 1, 7 |
| 39 | 4-1-2010: BOT 7 as BEACON and BOT 1 as EDGE worked extremely well. |
| 40 | 16 Performed Badly |
| 41 | 12 worked ok as beacon, not well as edge |
| 42 | |
| 43 | Fix orient code so the bot does not toggle back and forth when it tries to turn |
| 44 | |
| 45 | Use the center bot to check distances |
| 46 | Done--> Count them ("spam" method)
|
| 47 | Use beacon to find relative positions |
| 48 | Beacon tells them how to move to be at the right distance |
| 49 | *done*Wireless communication, initialization |
| 50 | */ |
| 51 | |
| 52 | int EDGE = 0; |
| 53 | int BEACON = 1; |
| 54 | int timeout = 0; |
| 55 | int sending = 0; |
| 56 | int stop2 = 0; |
| 57 | struct vector slave_position;
|
| 58 | int desired_max_bom;
|
| 59 | int bom_max_counter;
|
| 60 | |
| 61 | |
| 62 | void switch_sending(void) |
| 63 | {
|
| 64 | if(sending)
|
| 65 | {
|
| 66 | sending = 0;
|
| 67 | bom_off(); |
| 68 | } |
| 69 | else
|
| 70 | {
|
| 71 | sending = 1;
|
| 72 | bom_on(); |
| 73 | } |
| 74 | } |
| 75 | |
| 76 | void forward(int speed){ // set the motors to this forward speed. |
| 77 | motor_l_set(FORWARD,speed); |
| 78 | motor_r_set(FORWARD,speed); |
| 79 | } |
| 80 | void left(int speed){ // turn left at this speed. |
| 81 | motor_l_set(FORWARD,speed); |
| 82 | motor_r_set(BACKWARD,speed); |
| 83 | } |
| 84 | void right(int speed){ |
| 85 | motor_l_set(BACKWARD,speed); |
| 86 | motor_r_set(FORWARD,speed); |
| 87 | } |
| 88 | void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
| 89 | motor_l_set(BACKWARD,0); // stop() is better - motors_off() creates a slight delay to turn them back on. |
| 90 | motor_r_set(FORWARD,0);
|
| 91 | } |
| 92 | void setforward(int spd1, int spd2){ |
| 93 | motor_l_set(FORWARD,spd1); |
| 94 | motor_r_set(FORWARD,spd2); |
| 95 | } |
| 96 | void backward(int speed){ |
| 97 | motor_l_set(BACKWARD, speed); |
| 98 | motor_r_set(BACKWARD, speed); |
| 99 | } |
| 100 | int get_distance(void){ // takes an averaged reading of the front rangefinder |
| 101 | int temp,distance,kk=5; // kk sets this to 5 readings. |
| 102 | distance =0;
|
| 103 | for (int i=0; i<kk; i++){ |
| 104 | temp = range_read_distance(IR2); |
| 105 | if (temp == -1) |
| 106 | {
|
| 107 | //temp=0;
|
| 108 | i--; |
| 109 | } |
| 110 | else
|
| 111 | distance+= temp; |
| 112 | delay_ms(3);
|
| 113 | } |
| 114 | if (kk>0) |
| 115 | return (int)(distance/kk); |
| 116 | else
|
| 117 | return 0; |
| 118 | } |
| 119 | |
| 120 | /* Sends a global packet with two arguments */
|
| 121 | void send2(char arg0, char arg1) |
| 122 | {
|
| 123 | char send_buffer[2]; |
| 124 | send_buffer[0]=arg0;
|
| 125 | send_buffer[1]=arg1;
|
| 126 | wl_basic_send_global_packet(42,send_buffer,2); |
| 127 | } |
| 128 | |
| 129 | /* Sends a global packet with three arguments */
|
| 130 | void send3(char arg0, char arg1, char arg2) |
| 131 | {
|
| 132 | char send_buffer[3]; |
| 133 | send_buffer[0]=arg0;
|
| 134 | send_buffer[1]=arg1;
|
| 135 | send_buffer[2]=arg2;
|
| 136 | wl_basic_send_global_packet(42,send_buffer,2); |
| 137 | } |
| 138 | |
| 139 | /*
|
| 140 | Orients the robot so that it is facing the beacon (or the broadcasting BOM). |
| 141 | |
| 142 | */ |
| 143 | void correctTurn(void) |
| 144 | {
|
| 145 | orb1_set_color(BLUE); // BLUE and PURPLE
|
| 146 | left(220);
|
| 147 | while(1) |
| 148 | {
|
| 149 | int bomNum = 0; // bomNum is the current maximum reading |
| 150 | bom_refresh(BOM_ALL); |
| 151 | bomNum = bom_get_max(); |
| 152 | usb_puti(bomNum); |
| 153 | if(bomNum == 4) // when it's turned the right way, stop |
| 154 | {
|
| 155 | timeout = 0;
|
| 156 | //motor_l_set(1, 200);
|
| 157 | //motor_r_set(1, 200);
|
| 158 | break; // exits the while() loop to stop the method |
| 159 | } |
| 160 | else // facing the wrong way |
| 161 | {
|
| 162 | if(bomNum == -1) |
| 163 | {
|
| 164 | timeout++; |
| 165 | |
| 166 | if(timeout > 5000) // if it's been looking too long, move a little bit as it turns |
| 167 | {
|
| 168 | motor_r_set(FORWARD, 210);
|
| 169 | motor_l_set(BACKWARD, 190);
|
| 170 | } |
| 171 | } |
| 172 | else if((bomNum >= 12) || (bomNum < 4)) |
| 173 | {
|
| 174 | motor_l_set(FORWARD, 200);
|
| 175 | motor_r_set(BACKWARD, 200);
|
| 176 | timeout = 0;
|
| 177 | } |
| 178 | else
|
| 179 | {
|
| 180 | motor_l_set(BACKWARD, 200);
|
| 181 | motor_r_set(FORWARD, 200);
|
| 182 | timeout = 0;
|
| 183 | } |
| 184 | } |
| 185 | } |
| 186 | return;
|
| 187 | } |
| 188 | |
| 189 | |
| 190 | /*
|
| 191 | BLINK the given number times |
| 192 | */ |
| 193 | void blink(int num) |
| 194 | {
|
| 195 | for(int i = 0; i<num; i++) |
| 196 | {
|
| 197 | orb_set_color(ORB_OFF); |
| 198 | delay_ms(150);
|
| 199 | orb_set_color(RED); |
| 200 | delay_ms(50);
|
| 201 | } |
| 202 | orb_set_color(ORB_OFF); |
| 203 | } |
| 204 | |
| 205 | /*
|
| 206 | BLINK slowly the given number times |
| 207 | */ |
| 208 | void slowblink(int num) |
| 209 | {
|
| 210 | for(int i = 0; i<num; i++) |
| 211 | {
|
| 212 | orb_set_color(ORB_OFF); |
| 213 | delay_ms(300);
|
| 214 | orb_set_color(RED); |
| 215 | delay_ms(200);
|
| 216 | } |
| 217 | orb_set_color(ORB_OFF); |
| 218 | } |
| 219 | |
| 220 | |
| 221 | |
| 222 | |
| 223 | |
| 224 | |
| 225 | |
| 226 | //*****************************************************************************************************************************************************************************************
|
| 227 | |
| 228 | |
| 229 | |
| 230 | |
| 231 | |
| 232 | |
| 233 | /*
|
| 234 | A double state machine. The robot is either an "edge" or a "beacon" depending on the initial potentiometer reading. |
| 235 | */ |
| 236 | int main(void) |
| 237 | {
|
| 238 | /* Initialize dragonfly board */
|
| 239 | dragonfly_init(ALL_ON); |
| 240 | /* Initialize the basic wireless library */
|
| 241 | wl_basic_init_default(); |
| 242 | /* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ |
| 243 | wl_set_channel(24);
|
| 244 | |
| 245 | int robotid = get_robotid();
|
| 246 | int centerid = 0; |
| 247 | int used[17]; |
| 248 | for (int i=0; i<17; i++) used[i] = 0; |
| 249 | char send_buffer[2]; |
| 250 | int data_length;
|
| 251 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
| 252 | |
| 253 | |
| 254 | int state = EDGE;
|
| 255 | int beacon_State=0; |
| 256 | int edge_State=0; |
| 257 | int waitingCounter=0; |
| 258 | int robotsReceived=0; |
| 259 | int offset = 20; |
| 260 | int time=0; |
| 261 | int direction = 4; // keeps track of which way robots are facing relative to the center |
| 262 | |
| 263 | if(wheel()>100) |
| 264 | {
|
| 265 | state=BEACON; |
| 266 | } |
| 267 | |
| 268 | int distance=1000; // how far away to stop. |
| 269 | int onefoot=250, speed=250; // one foot is 490 for bot 1; one foot is 200 for bot6 |
| 270 | |
| 271 | while(1) |
| 272 | {
|
| 273 | bom_refresh(BOM_ALL); |
| 274 | |
| 275 | /*
|
| 276 | *******TERMinology************** |
| 277 | EDGE=0 other names: slave. Definition: robots on the edge of the circle; |
| 278 | BEACON=1 other name: master. Definition: robots in the center of the circle; |
| 279 | |
| 280 | |
| 281 | *******EXPECTED MOVES ********** |
| 282 | The designed movement: |
| 283 | 1. one center robot, several edge robots are on; |
| 284 | 2. center robots: button 1 is pressed; |
| 285 | 3. center robots: send global package telling edges that he exists; |
| 286 | 4. EDGE robots response with ACK. |
| 287 | 5. EDGE robots wait for center robots to finish counting (DONE package) |
| 288 | 6. EDGE robtos approach the center robtot and stop at the "onefoot" distance, send message to the center |
| 289 | */ |
| 290 | |
| 291 | |
| 292 | // decide if its is center or not. This is the main switch loop which governs the entire state of the robot.
|
| 293 | switch(state)
|
| 294 | {
|
| 295 | /**********
|
| 296 | if EDGE /slave robots |
| 297 | */ |
| 298 | case 0: |
| 299 | |
| 300 | switch(edge_State)
|
| 301 | {
|
| 302 | /*
|
| 303 | 0. EDGE robots are on. |
| 304 | 1. They are waiting for EXIST pacakage from the Center robots |
| 305 | 2. After they receive the package, they send ACK package to center. |
| 306 | 3. Done for now: display green. |
| 307 | */ |
| 308 | case 0: |
| 309 | bom_off(); |
| 310 | orb1_set_color(YELLOW); |
| 311 | orb2_set_color(CYAN); |
| 312 | packet_data=wl_basic_do_default(&data_length); |
| 313 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
| 314 | {
|
| 315 | centerid = packet_data[1];
|
| 316 | |
| 317 | send2(CIRCLE_ACTION_ACK,robotid); |
| 318 | |
| 319 | edge_State=1;
|
| 320 | } |
| 321 | break;
|
| 322 | /*
|
| 323 | 1. Wait for DONE package |
| 324 | 2. The counting process is DONE |
| 325 | */ |
| 326 | case 1: |
| 327 | |
| 328 | orb_set_color(YELLOW); |
| 329 | orb2_set_color(PURPLE); |
| 330 | |
| 331 | send2(CIRCLE_ACTION_ACK,robotid); // keep sending the packet until we get a response
|
| 332 | |
| 333 | packet_data=wl_basic_do_default(&data_length); |
| 334 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid) |
| 335 | {
|
| 336 | edge_State=2;
|
| 337 | } |
| 338 | break;
|
| 339 | |
| 340 | case 2: // wait for the second, general, done packet. |
| 341 | |
| 342 | orb_set_color(YELLOW); |
| 343 | packet_data=wl_basic_do_default(&data_length); |
| 344 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE && packet_data[1] == centerid) |
| 345 | {
|
| 346 | edge_State=3;
|
| 347 | } |
| 348 | break;
|
| 349 | |
| 350 | case 3: |
| 351 | // COLOR afer DONE ---> MAGENTA
|
| 352 | orb_set_color(MAGENTA); |
| 353 | correctTurn(); // turn to face the beacon
|
| 354 | forward(175);
|
| 355 | //range_init();
|
| 356 | |
| 357 | |
| 358 | distance = get_distance(); |
| 359 | time=0;
|
| 360 | while ((distance-offset)>=onefoot || distance==0 || (distance+offset)<onefoot) |
| 361 | {
|
| 362 | if(distance==0) |
| 363 | orb_set_color(WHITE); |
| 364 | else if(distance-offset>=onefoot) |
| 365 | forward(175);
|
| 366 | else
|
| 367 | backward(175);
|
| 368 | //correctApproach();
|
| 369 | distance = get_distance(); |
| 370 | delay_ms(14);
|
| 371 | time+=14;
|
| 372 | if(time>500) |
| 373 | {
|
| 374 | correctTurn(); |
| 375 | time=0;
|
| 376 | } |
| 377 | } |
| 378 | |
| 379 | stop(); |
| 380 | orb_set_color(GREEN); |
| 381 | |
| 382 | send2(CIRCLE_ACTION_ACK, robotid); |
| 383 | |
| 384 | stop(); |
| 385 | edge_State=4;
|
| 386 | break;
|
| 387 | |
| 388 | // waits for a packet to tell it to turn on the bom.
|
| 389 | case 4: |
| 390 | packet_data=wl_basic_do_default(&data_length); |
| 391 | if(packet_data != 0 && data_length==2 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid) |
| 392 | {
|
| 393 | bom_on(); |
| 394 | orb_set_color(ORANGE); |
| 395 | send2(CIRCLE_ACTION_ACK,centerid); |
| 396 | edge_State = 5;
|
| 397 | } |
| 398 | break;
|
| 399 | |
| 400 | // waits for a packet to tell it that it has been received.
|
| 401 | case 5: |
| 402 | orb2_set_color(YELLOW); |
| 403 | packet_data=wl_basic_do_default(&data_length); |
| 404 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_GOTYOU && packet_data[1] == robotid) |
| 405 | {
|
| 406 | bom_off(); |
| 407 | direction = packet_data[2];
|
| 408 | orb_set_color(YELLOW); |
| 409 | edge_State = 6;
|
| 410 | } |
| 411 | break;
|
| 412 | |
| 413 | /* Blinks the direction it should turn. */
|
| 414 | case 6: |
| 415 | slowblink(direction); |
| 416 | edge_State = 7;
|
| 417 | break;
|
| 418 | |
| 419 | case 7: |
| 420 | orb_set_color(GREEN); |
| 421 | break;
|
| 422 | |
| 423 | |
| 424 | } // end the EdgeState switch
|
| 425 | |
| 426 | break; // break the Edge state in the main switch loop |
| 427 | |
| 428 | // END for EDGE robots
|
| 429 | |
| 430 | |
| 431 | |
| 432 | |
| 433 | |
| 434 | |
| 435 | |
| 436 | //*****************************************************************************************************************************************************************************************
|
| 437 | //*****************************************************************************************************************************************************************************************
|
| 438 | //*****************************************************************************************************************************************************************************************
|
| 439 | |
| 440 | |
| 441 | |
| 442 | |
| 443 | |
| 444 | /*
|
| 445 | The CENTER/BEACON/MASTER robot |
| 446 | */ |
| 447 | case 1: |
| 448 | switch(beacon_State)
|
| 449 | {
|
| 450 | |
| 451 | /* 0. center robots on wait for pressing button 1 */
|
| 452 | case 0: |
| 453 | bom_on(); |
| 454 | orb_set_color(PURPLE); |
| 455 | if(button1_click()) beacon_State=1; |
| 456 | break;
|
| 457 | |
| 458 | /* 1. Send EXIST package to EDGE robots */
|
| 459 | case 1: |
| 460 | orb_set_color(RED); |
| 461 | send2(CIRCLE_ACTION_EXIST,robotid); |
| 462 | beacon_State=2;
|
| 463 | break;
|
| 464 | |
| 465 | /* 2. Count the number of the EDGE robots *******NOTE: at most 1000 times of loop ****** */
|
| 466 | case 2: |
| 467 | waitingCounter++; |
| 468 | orb1_set_color(YELLOW); |
| 469 | orb2_set_color(BLUE); |
| 470 | packet_data=wl_basic_do_default(&data_length); |
| 471 | |
| 472 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
| 473 | {
|
| 474 | orb_set_color(RED); |
| 475 | orb2_set_color(BLUE); |
| 476 | //only add to robots seen if you haven't gotten an ACK from this robot
|
| 477 | if(used[packet_data[1]]==0) |
| 478 | {
|
| 479 | robotsReceived++; |
| 480 | used[packet_data[1]] = 1; |
| 481 | |
| 482 | usb_puts("Added: ");
|
| 483 | usb_puti(packet_data[1]);
|
| 484 | usb_puts("\r\n");
|
| 485 | } |
| 486 | |
| 487 | // NEW: sends a packet to each robot it receives telling them to be done.
|
| 488 | send2(CIRCLE_ACTION_GOTYOU,packet_data[1]);
|
| 489 | } |
| 490 | if(waitingCounter >= 300){ |
| 491 | beacon_State=3;
|
| 492 | } |
| 493 | break;
|
| 494 | |
| 495 | /* COUNTing is DONE. Sending DONE package. */
|
| 496 | case 3: |
| 497 | blink(robotsReceived); |
| 498 | orb_set_color(GREEN); |
| 499 | send2(CIRCLE_ACTION_DONE, robotid); |
| 500 | beacon_State=4;
|
| 501 | break;
|
| 502 | |
| 503 | /* Wait for all the robots to get to right distance/position */
|
| 504 | case 4: |
| 505 | left(170);
|
| 506 | orb1_set_color(YELLOW); |
| 507 | orb2_set_color(WHITE); |
| 508 | |
| 509 | int numOk = 0; |
| 510 | |
| 511 | while(numOk<robotsReceived)
|
| 512 | {
|
| 513 | packet_data=wl_basic_do_default(&data_length); |
| 514 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
| 515 | {
|
| 516 | numOk++; |
| 517 | } |
| 518 | } |
| 519 | |
| 520 | beacon_State = 5;
|
| 521 | break;
|
| 522 | |
| 523 | /* Turns all the robots in the same direction */
|
| 524 | case 5: |
| 525 | stop(); |
| 526 | bom_off(); |
| 527 | orb_set_color(ORANGE); |
| 528 | |
| 529 | // for each robot, tells them to turn their bom on, then tells them which way they should face.
|
| 530 | for(int i=0; i < 17; i++) |
| 531 | {
|
| 532 | if(used[i] == 1) |
| 533 | {
|
| 534 | send2(CIRCLE_ACTION_GOTYOU, i); |
| 535 | while(1) // waits for a response so it knows the BOM is on. |
| 536 | {
|
| 537 | orb_set_color(RED); |
| 538 | orb2_set_color(WHITE); |
| 539 | packet_data=wl_basic_do_default(&data_length); |
| 540 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
| 541 | {
|
| 542 | orb_set_color(ORANGE); |
| 543 | break;
|
| 544 | } |
| 545 | } |
| 546 | |
| 547 | direction = bom_get_max(); |
| 548 | if(direction >= 16) direction -= 16; |
| 549 | send3(CIRCLE_ACTION_GOTYOU, i, direction); |
| 550 | delay_ms(2000);
|
| 551 | } |
| 552 | } |
| 553 | beacon_State = 6;
|
| 554 | break;
|
| 555 | |
| 556 | /* Terminal. */
|
| 557 | case 6: |
| 558 | orb_set_color(GREEN); |
| 559 | break;
|
| 560 | } |
| 561 | break;
|
| 562 | |
| 563 | } |
| 564 | } |
| 565 | |
| 566 | orb_set_color(RED); |
| 567 | while(1); /* END HERE */ |
| 568 | |
| 569 | //return 0;
|
| 570 | } |