root / trunk / code / behaviors / formation_control / circle / circle.c @ 1626
History | View | Annotate | Download (13.6 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 | void switch_sending ()
|
||
| 37 | {
|
||
| 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 | 1618 | jmcarrol | int correctTurn(void) |
| 130 | {
|
||
| 131 | int bomNum = 0; |
||
| 132 | bom_refresh(BOM_ALL); |
||
| 133 | bomNum = bom_get_max(); |
||
| 134 | usb_puti(bomNum); |
||
| 135 | if(bomNum == 4) |
||
| 136 | { timeout = 0;
|
||
| 137 | 1626 | gnagaraj | //motor_l_set(1, 200);
|
| 138 | //motor_r_set(1, 200);
|
||
| 139 | return 1; //<--------------------------------------------1 and 0 are switched |
||
| 140 | 1618 | jmcarrol | } |
| 141 | else
|
||
| 142 | {
|
||
| 143 | if(bomNum == -1){ |
||
| 144 | timeout++; |
||
| 145 | if(timeout > 500000) |
||
| 146 | {
|
||
| 147 | motor_r_set(FORWARD, 210);
|
||
| 148 | motor_l_set(BACKWARD, 190);
|
||
| 149 | } |
||
| 150 | } |
||
| 151 | else if((bomNum >= 12) || (bomNum < 4)) |
||
| 152 | {
|
||
| 153 | motor_l_set(FORWARD, 200);timeout = 0; |
||
| 154 | motor_r_set(BACKWARD, 200);
|
||
| 155 | } |
||
| 156 | else
|
||
| 157 | {
|
||
| 158 | motor_l_set(BACKWARD, 200);timeout = 0; |
||
| 159 | motor_r_set(FORWARD, 200);
|
||
| 160 | } |
||
| 161 | } |
||
| 162 | 1626 | gnagaraj | return 0; |
| 163 | 1618 | jmcarrol | } |
| 164 | |||
| 165 | 1626 | gnagaraj | |
| 166 | /*~~~~~~~~~~~~~~ NEED document
|
||
| 167 | */
|
||
| 168 | 1618 | jmcarrol | void correctApproach(void) |
| 169 | {
|
||
| 170 | int bomNum = 0; |
||
| 171 | bom_refresh(BOM_ALL); |
||
| 172 | bomNum = bom_get_max(); |
||
| 173 | usb_puti(bomNum); |
||
| 174 | if(bomNum == 4) |
||
| 175 | {
|
||
| 176 | motor_l_set(1, 200); |
||
| 177 | motor_r_set(1, 200); |
||
| 178 | } |
||
| 179 | else
|
||
| 180 | {
|
||
| 181 | if(bomNum == -1){} |
||
| 182 | else if((bomNum >= 12) || (bomNum < 4)) |
||
| 183 | {
|
||
| 184 | motor_l_set(FORWARD, 200);
|
||
| 185 | motor_r_set(BACKWARD, 200);
|
||
| 186 | } |
||
| 187 | else
|
||
| 188 | {
|
||
| 189 | motor_l_set(BACKWARD, 200);
|
||
| 190 | motor_r_set(FORWARD, 200);
|
||
| 191 | } |
||
| 192 | delay_ms(100);
|
||
| 193 | } |
||
| 194 | } |
||
| 195 | |||
| 196 | 1626 | gnagaraj | |
| 197 | /*
|
||
| 198 | Start turning until the front receive the MAx bom reading
|
||
| 199 | */
|
||
| 200 | 1594 | azirbel | void turn_to_beacon(int max){ |
| 201 | if (max>-1 && max<16){ |
||
| 202 | int index = (max+12)%16; |
||
| 203 | if (index==0) { |
||
| 204 | stop(); |
||
| 205 | } |
||
| 206 | else if (index<8) right(170); |
||
| 207 | else left(170); |
||
| 208 | } |
||
| 209 | } |
||
| 210 | |||
| 211 | 1626 | gnagaraj | |
| 212 | /*
|
||
| 213 | Start turning its fron to the MAx bom reading
|
||
| 214 | But it won't stop by itself.
|
||
| 215 | */
|
||
| 216 | 1594 | azirbel | void turn_to_beacon2(int max){ // like the previous but no stop() call' |
| 217 | |||
| 218 | |||
| 219 | |||
| 220 | |||
| 221 | if (max>-1 && max<16){ |
||
| 222 | int index = (max+12)%16; |
||
| 223 | if (index==0) { |
||
| 224 | |||
| 225 | } |
||
| 226 | else if (index<8) right(170); |
||
| 227 | else left(170); |
||
| 228 | } |
||
| 229 | } |
||
| 230 | 1618 | jmcarrol | |
| 231 | 1626 | gnagaraj | |
| 232 | |||
| 233 | |||
| 234 | /*
|
||
| 235 | Turn towards the MAX bom direction
|
||
| 236 | until the front receive the MAx bom reading
|
||
| 237 | */
|
||
| 238 | 1594 | azirbel | void orient(void){ |
| 239 | int max_index = -1; |
||
| 240 | while (max_index!=4) { |
||
| 241 | /* Refresh and make sure the table is updated */
|
||
| 242 | bom_refresh(BOM_ALL); |
||
| 243 | max_index = bom_get_max(); |
||
| 244 | turn_to_beacon(max_index); |
||
| 245 | delay_ms(22);
|
||
| 246 | } |
||
| 247 | } |
||
| 248 | 1626 | gnagaraj | |
| 249 | |||
| 250 | /*
|
||
| 251 | Turn towards the MAX bom direction
|
||
| 252 | until the front receive the MAx bom reading
|
||
| 253 | |||
| 254 | This function has less stop() so the robots run more smooth
|
||
| 255 | */
|
||
| 256 | 1594 | azirbel | void orient2(void){ |
| 257 | int max_index = -1; |
||
| 258 | while (max_index!=4) { |
||
| 259 | /* Refresh and make sure the table is updated */
|
||
| 260 | bom_refresh(BOM_ALL); |
||
| 261 | max_index = bom_get_max(); |
||
| 262 | turn_to_beacon2(max_index); |
||
| 263 | delay_ms(22);
|
||
| 264 | } |
||
| 265 | } |
||
| 266 | 1618 | jmcarrol | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
| 267 | 1594 | azirbel | void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
| 268 | forward(200);
|
||
| 269 | encoder_rst_dx(LEFT); |
||
| 270 | encoder_rst_dx(RIGHT); |
||
| 271 | delay_ms(100);
|
||
| 272 | int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
|
||
| 273 | int count = 0; |
||
| 274 | int d;
|
||
| 275 | while (count<25){ //count = 25 when bot6; count <12 |
||
| 276 | x_left = encoder_get_x(LEFT); |
||
| 277 | x_right = encoder_get_x(RIGHT); |
||
| 278 | d = x_right-x_left; |
||
| 279 | if (d>13 || d<-13){ |
||
| 280 | if(d<50 && d>-50){ |
||
| 281 | d = round(1.0*d/4); |
||
| 282 | setforward(200+d, 200-d); |
||
| 283 | } |
||
| 284 | } |
||
| 285 | delay_ms(32);
|
||
| 286 | count++; |
||
| 287 | } |
||
| 288 | } |
||
| 289 | |||
| 290 | 1626 | gnagaraj | |
| 291 | /*
|
||
| 292 | BLINK the given number times
|
||
| 293 | */
|
||
| 294 | 1594 | azirbel | void blink(int num) { |
| 295 | for(int i = 0; i<num; i++) |
||
| 296 | {
|
||
| 297 | orb_set_color(ORB_OFF); |
||
| 298 | 1610 | azirbel | delay_ms(200);
|
| 299 | 1594 | azirbel | orb_set_color(RED); |
| 300 | 1610 | azirbel | delay_ms(200);
|
| 301 | 1594 | azirbel | } |
| 302 | orb_set_color(ORB_OFF); |
||
| 303 | } |
||
| 304 | |||
| 305 | |||
| 306 | |||
| 307 | 1626 | gnagaraj | |
| 308 | |||
| 309 | 1594 | azirbel | int main(void) |
| 310 | {
|
||
| 311 | /* Initialize dragonfly board */
|
||
| 312 | 1618 | jmcarrol | dragonfly_init(ALL_ON); |
| 313 | /* Initialize the basic wireless library */
|
||
| 314 | wl_basic_init_default(); |
||
| 315 | /* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ |
||
| 316 | 1594 | azirbel | wl_set_channel(24);
|
| 317 | |||
| 318 | 1626 | gnagaraj | int sending_counter = 0; |
| 319 | 1594 | azirbel | int robotid = get_robotid();
|
| 320 | int used[16]; |
||
| 321 | 1597 | azirbel | for (int i=0; i<16; i++) used[i] = 0; |
| 322 | 1594 | azirbel | char send_buffer[2]; |
| 323 | int data_length;
|
||
| 324 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
||
| 325 | |||
| 326 | |||
| 327 | int state = EDGE;
|
||
| 328 | int beacon_State=0; |
||
| 329 | 1610 | azirbel | int waitingCounter=0; |
| 330 | int robotsReceived=0; |
||
| 331 | 1594 | azirbel | if(wheel()<100) |
| 332 | {
|
||
| 333 | state=EDGE; |
||
| 334 | } |
||
| 335 | else
|
||
| 336 | {
|
||
| 337 | state=BEACON; |
||
| 338 | } |
||
| 339 | |||
| 340 | int distance=1000; // how far away to stop. |
||
| 341 | 1610 | azirbel | int onefoot=300, speed=250; // one foot is 490 for bot 1; one foot is 200 for bot6 |
| 342 | 1594 | azirbel | |
| 343 | 1596 | azirbel | while(1) |
| 344 | {
|
||
| 345 | 1626 | gnagaraj | bom_refresh(BOM_ALL); |
| 346 | |||
| 347 | /*
|
||
| 348 | *******TERMinology**************
|
||
| 349 | EDGE=0 other names: slave. Definition: robots on the edge of the circle;
|
||
| 350 | BEACON=1 other name: master. Definition: robots in the center of the circle;
|
||
| 351 | |||
| 352 | |||
| 353 | *******EXPECTED MOVES **********
|
||
| 354 | The designed movement:
|
||
| 355 | 1. one center robot, several edge robots are on;
|
||
| 356 | 2. center robots: button 1 is pressed;
|
||
| 357 | 3. center robots: send global package telling edges that he exists;
|
||
| 358 | 4. EDGE robots response with ACK.
|
||
| 359 | 5. EDGE robots wait for center robots to finish counting (DONE package)
|
||
| 360 | 6. *******************TODO ***************
|
||
| 361 | */
|
||
| 362 | |||
| 363 | |||
| 364 | // decide if its is center or not.
|
||
| 365 | 1596 | azirbel | switch(state)
|
| 366 | {
|
||
| 367 | 1626 | gnagaraj | /**********
|
| 368 | if EDGE /slave robots
|
||
| 369 | */
|
||
| 370 | case 0: |
||
| 371 | 1594 | azirbel | |
| 372 | 1626 | gnagaraj | /*
|
| 373 | 0. EDGE robots are on.
|
||
| 374 | 1. They are waiting for ExiST pacakage from the Center robots
|
||
| 375 | 2. After they receive the package, they send ACK package to center.
|
||
| 376 | 3. Wait the center robot to finish counting all EDGE robots
|
||
| 377 | */
|
||
| 378 | while(1) |
||
| 379 | 1594 | azirbel | {
|
| 380 | 1626 | gnagaraj | bom_off(); |
| 381 | 1618 | jmcarrol | orb1_set_color(YELLOW);orb2_set_color(CYAN); |
| 382 | 1594 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 383 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
||
| 384 | {
|
||
| 385 | send_buffer[0]=CIRCLE_ACTION_ACK;
|
||
| 386 | send_buffer[1]=robotid;
|
||
| 387 | |||
| 388 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 389 | break;
|
||
| 390 | } |
||
| 391 | } |
||
| 392 | |||
| 393 | 1626 | gnagaraj | /*
|
| 394 | 1. Wait for DONE package
|
||
| 395 | 2. The counting process is DONE
|
||
| 396 | */
|
||
| 397 | while(1) |
||
| 398 | 1596 | azirbel | {
|
| 399 | 1618 | jmcarrol | orb_set_color(YELLOW);orb2_set_color(PURPLE); |
| 400 | 1594 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 401 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 402 | 1596 | azirbel | |
| 403 | 1594 | azirbel | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) |
| 404 | {
|
||
| 405 | break;
|
||
| 406 | } |
||
| 407 | } |
||
| 408 | |||
| 409 | 1626 | gnagaraj | |
| 410 | // COLOR afer DONE ---> MAGENTA
|
||
| 411 | 1618 | jmcarrol | orb_set_color(MAGENTA); |
| 412 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||
| 413 | while(correctTurn()){
|
||
| 414 | } |
||
| 415 | 1626 | gnagaraj | forward(220);
|
| 416 | 1594 | azirbel | //range_init();
|
| 417 | |||
| 418 | |||
| 419 | distance = get_distance(); |
||
| 420 | while (distance>=onefoot || distance==0) |
||
| 421 | {
|
||
| 422 | if(distance==0) |
||
| 423 | orb_set_color(WHITE); |
||
| 424 | 1618 | jmcarrol | //correctApproach();
|
| 425 | 1594 | azirbel | distance = get_distance(); |
| 426 | delay_ms(14);
|
||
| 427 | } |
||
| 428 | 1618 | jmcarrol | |
| 429 | 1594 | azirbel | stop(); |
| 430 | 1618 | jmcarrol | orb_set_color(LIME); |
| 431 | 1594 | azirbel | |
| 432 | 1626 | gnagaraj | send_buffer[0]=CIRCLE_ACTION_ACK;
|
| 433 | send_buffer[1]=robotid;
|
||
| 434 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 435 | 1594 | azirbel | |
| 436 | 1626 | gnagaraj | // NEW CODE FROM JOEL
|
| 437 | 1596 | azirbel | |
| 438 | 1626 | gnagaraj | while(1) |
| 439 | {
|
||
| 440 | packet_data = wl_basic_do_default(&data_length); |
||
| 441 | |||
| 442 | if(packet_data[0]=='s') stop2=1; |
||
| 443 | if(packet_data[0]=='a') switch_sending(); |
||
| 444 | |||
| 445 | if(sending)
|
||
| 446 | {
|
||
| 447 | |||
| 448 | } |
||
| 449 | 1596 | azirbel | |
| 450 | 1626 | gnagaraj | else // recieving |
| 451 | {
|
||
| 452 | |||
| 453 | if(stop2)
|
||
| 454 | {
|
||
| 455 | motor_l_set(FORWARD,0);
|
||
| 456 | motor_r_set(FORWARD,0);
|
||
| 457 | orb1_set_color(GREEN); |
||
| 458 | } |
||
| 459 | |||
| 460 | else
|
||
| 461 | {
|
||
| 462 | int max_bom = bom_get_max();
|
||
| 463 | /*usb_puts("bom_get_max : ");
|
||
| 464 | usb_puti(max_bom);
|
||
| 465 | usb_puts("/n/r");*/
|
||
| 466 | |||
| 467 | |||
| 468 | if(max_bom == 8) |
||
| 469 | {
|
||
| 470 | orb2_set_color(BLUE); |
||
| 471 | motor_r_set(FORWARD,180);
|
||
| 472 | motor_l_set(FORWARD,180);
|
||
| 473 | |||
| 474 | } |
||
| 475 | else if(max_bom == 7 || max_bom == 6 || max_bom == 5) |
||
| 476 | {
|
||
| 477 | motor_l_set(FORWARD,180);
|
||
| 478 | motor_r_set(FORWARD,0);
|
||
| 479 | } |
||
| 480 | else if(max_bom == -1); |
||
| 481 | else
|
||
| 482 | {
|
||
| 483 | orb2_set_color(GREEN); |
||
| 484 | motor_l_set(FORWARD,0);
|
||
| 485 | motor_r_set(FORWARD,180);
|
||
| 486 | } |
||
| 487 | } |
||
| 488 | |||
| 489 | } |
||
| 490 | |||
| 491 | delay_ms(10);
|
||
| 492 | |||
| 493 | } //end while
|
||
| 494 | break;
|
||
| 495 | |||
| 496 | // END for EDGE robots
|
||
| 497 | |||
| 498 | |||
| 499 | |||
| 500 | |||
| 501 | |||
| 502 | |||
| 503 | /***************
|
||
| 504 | if The CENTER/BEACON/MASTER robot
|
||
| 505 | */
|
||
| 506 | case 1: // BEACON /master /enter robots |
||
| 507 | 1594 | azirbel | switch(beacon_State) {
|
| 508 | 1626 | gnagaraj | /*
|
| 509 | 1. center robots on wait for pressing button 1
|
||
| 510 | */
|
||
| 511 | case 0: |
||
| 512 | 1594 | azirbel | bom_on(); |
| 513 | orb_set_color(PURPLE); |
||
| 514 | if(button1_click()) beacon_State=1; |
||
| 515 | 1626 | gnagaraj | break;
|
| 516 | /*
|
||
| 517 | 1. Send EXIST package to EDGE robots
|
||
| 518 | */
|
||
| 519 | 1596 | azirbel | case 1: // sends a global exist packet to see how many robots there are |
| 520 | 1618 | jmcarrol | orb_set_color(RED); |
| 521 | 1594 | azirbel | send_buffer[0]=CIRCLE_ACTION_EXIST;
|
| 522 | send_buffer[1]=get_robotid();
|
||
| 523 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 524 | beacon_State=2;
|
||
| 525 | 1626 | gnagaraj | break;
|
| 526 | /*
|
||
| 527 | 1. Count the number of the EDGE robots
|
||
| 528 | *******NOTE: at most 1500 times of loop ******
|
||
| 529 | */
|
||
| 530 | case 2: |
||
| 531 | waitingCounter++; |
||
| 532 | orb1_set_color(YELLOW); |
||
| 533 | orb2_set_color(BLUE); |
||
| 534 | packet_data=wl_basic_do_default(&data_length); |
||
| 535 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
||
| 536 | {
|
||
| 537 | orb_set_color(RED);orb2_set_color(BLUE); |
||
| 538 | //only add to robots seen if you haven't gotten an ACK from this robot
|
||
| 539 | if(used[packet_data[1]]==0){ |
||
| 540 | robotsReceived++; |
||
| 541 | used[packet_data[1]]=1; |
||
| 542 | } |
||
| 543 | 1594 | azirbel | } |
| 544 | 1626 | gnagaraj | if(waitingCounter >= 1500){ |
| 545 | beacon_State=3;
|
||
| 546 | } |
||
| 547 | break;
|
||
| 548 | /*
|
||
| 549 | COUNTing is DONE.
|
||
| 550 | Sending DONE package.
|
||
| 551 | */
|
||
| 552 | 1594 | azirbel | case 3: |
| 553 | 1626 | gnagaraj | orb_set_color(GREEN); |
| 554 | send_buffer[0]=CIRCLE_ACTION_DONE;
|
||
| 555 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 556 | beacon_State=4;
|
||
| 557 | break;
|
||
| 558 | /*
|
||
| 559 | Wait for all the robots to get to right distance/position
|
||
| 560 | */
|
||
| 561 | case 4: |
||
| 562 | orb1_set_color(YELLOW); |
||
| 563 | orb2_set_color(WHITE); |
||
| 564 | |||
| 565 | int numOk = 0; |
||
| 566 | |||
| 567 | while(numOk<robotsReceived)
|
||
| 568 | {
|
||
| 569 | packet_data=wl_basic_do_default(&data_length); |
||
| 570 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
||
| 571 | {
|
||
| 572 | numOk++; |
||
| 573 | } |
||
| 574 | } |
||
| 575 | |||
| 576 | beacon_State = 5;
|
||
| 577 | break; // <----------------------------------------------------------Echo wrote this "break". need to be checked. |
||
| 578 | |||
| 579 | /**
|
||
| 580 | NEED to be documented
|
||
| 581 | */
|
||
| 582 | case 5: |
||
| 583 | orb_set_color(BLUE); |
||
| 584 | // clock for switching the BOMs between the master and slave
|
||
| 585 | if(sending_counter++>4) |
||
| 586 | {
|
||
| 587 | switch_sending(); |
||
| 588 | sending_counter = 0;
|
||
| 589 | send_buffer[0] = 'a'; |
||
| 590 | wl_basic_send_global_packet(42, send_buffer, 1); |
||
| 591 | } |
||
| 592 | |||
| 593 | |||
| 594 | if(sending)
|
||
| 595 | {
|
||
| 596 | |||
| 597 | } |
||
| 598 | |||
| 599 | else // recieving |
||
| 600 | {
|
||
| 601 | int max_bom = bom_get_max();
|
||
| 602 | usb_puts("bom_get_max : ");
|
||
| 603 | usb_puti(max_bom); |
||
| 604 | usb_puts("\n\r");
|
||
| 605 | |||
| 606 | if(max_bom == desired_max_bom)
|
||
| 607 | {
|
||
| 608 | // only stops the slave if two consecutive boms
|
||
| 609 | // reading give the desired bom as the max one. Filters the noise.
|
||
| 610 | if(bom_max_counter)
|
||
| 611 | {
|
||
| 612 | send_buffer[0] = 's'; |
||
| 613 | wl_basic_send_global_packet(42, send_buffer, 2); |
||
| 614 | } |
||
| 615 | bom_max_counter =1;
|
||
| 616 | } |
||
| 617 | else bom_max_counter = 0; |
||
| 618 | |||
| 619 | } |
||
| 620 | |||
| 621 | break;
|
||
| 622 | } |
||
| 623 | 1594 | azirbel | } |
| 624 | } |
||
| 625 | |||
| 626 | orb_set_color(RED); |
||
| 627 | while(1); /* END HERE */ |
||
| 628 | |||
| 629 | 1626 | gnagaraj | //return 0;
|
| 630 | 1594 | azirbel | } |