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