## Revision 1635

Debugged circle.c so approaching works

View differences:

circle.c
12 12
```int desired_max_bom;
```
13 13
```int bom_max_counter;
```
14 14

15

15
```//Last used 12,13,7(BOM)
```
16 16
```void set_desired_max_bom(int desired_angle)
```
17 17
```{
```
18 18
```	if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0;
```
......
269 269
```
```
270 270
```	int state = EDGE;
```
271 271
```	int beacon_State=0;
```
272
```	int edge_State=0;
```
272 273
```	int waitingCounter=0;
```
273 274
```	int robotsReceived=0;
```
275
```	int offset = 20, time=0;
```
274 276
```
```
275 277
```	if(wheel()<100)
```
276 278
```	{
```
......
282 284
```	}
```
283 285
```
```
284 286
```	int distance=1000;						// how far away to stop.
```
285
```	int onefoot=300, speed=250;				// one foot is 490 for bot 1; one foot is 200 for bot6
```
287
```	int onefoot=250, speed=250;				// one foot is 490 for bot 1; one foot is 200 for bot6
```
286 288
```
```
287 289
```	while(1)
```
288 290
```	{
```
......
312 314
```				if  EDGE /slave robots
```
313 315
```			*/
```
314 316
```			case 0:
```
315
```
```
316
```				/*
```
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)
```
317
```				switch(edge_State)
```
323 318
```				{
```
324
```					bom_off();
```
325
```					orb1_set_color(YELLOW);orb2_set_color(CYAN);
```
326
```					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;
```
319
```					/*
```
320
```						0. EDGE robots are on.
```
321
```						1. They are waiting for ExiST pacakage from the Center robots
```
322
```						2. After they receive the package, they send ACK package to center.
```
323
```						3. Wait the center robot to finish counting all EDGE robots
```
324
```					*/
```
325
```					case 0:
```
326
```						bom_off();
```
327
```						orb1_set_color(YELLOW);orb2_set_color(CYAN);
```
328
```						packet_data=wl_basic_do_default(&data_length);
```
329
```						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
```
330
```						{
```
331
```							send_buffer[0]=CIRCLE_ACTION_ACK;
```
332
```							send_buffer[1]=robotid;
```
333
```
```
334
```							wl_basic_send_global_packet(42,send_buffer,2);
```
335
```							edge_State=1;
```
336
```						}
```
337
```					break;
```
338
```					/*
```
339
```						1. Wait for DONE package
```
340
```						2. The counting process is DONE
```
341
```					*/
```
342
```					case 1:
```
343
```
```
344
```						orb_set_color(YELLOW);orb2_set_color(PURPLE);
```
345
```						packet_data=wl_basic_do_default(&data_length);
```
346
```						//wl_basic_send_global_packet(42,send_buffer,2);
```
331 347
```
```
332
```						wl_basic_send_global_packet(42,send_buffer,2);
```
333
```						break;
```
334
```					}
```
335
```				}
```
336
```
```
337
```				/*
```
338
```					1. Wait for DONE package
```
339
```					2. The counting process is DONE
```
340
```				*/
```
341
```				while(1)
```
342
```				{
```
343
```					orb_set_color(YELLOW);orb2_set_color(PURPLE);
```
344
```					packet_data=wl_basic_do_default(&data_length);
```
345
```					wl_basic_send_global_packet(42,send_buffer,2);
```
348
```						if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
```
349
```						{
```
350
```							edge_State=2;
```
351
```						}
```
352
```					break;
```
346 353
```
```
347
```					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
```
348
```					{
```
349
```						break;
```
350
```					}
```
354
```					case 2:
```
355
```						// COLOR afer DONE ---> MAGENTA
```
356
```						orb_set_color(MAGENTA);
```
357
```						correctTurn();			// turn to face the beacon
```
358
```						forward(175);
```
359
```						//range_init();
```
360
```
```
361
```
```
362
```						distance = get_distance();
```
363
```						time=0;
```
364
```						while ((distance-offset)>=onefoot || distance==0 || (distance+offset)<onefoot)
```
365
```						{
```
366
```							if(distance==0)
```
367
```								orb_set_color(WHITE);
```
368
```							else if(distance-offset>=onefoot)
```
369
```								forward(175);
```
370
```							else
```
371
```								backward(175);
```
372
```							//correctApproach();
```
373
```							distance = get_distance();
```
374
```							delay_ms(14);
```
375
```							time+=14;
```
376
```							if(time>500){
```
377
```								correctTurn();
```
378
```								time=0;
```
379
```							}
```
380
```						}
```
381
```
```
382
```						stop();
```
383
```						orb_set_color(LIME);
```
384
```						edge_State=3;
```
385
```					break;
```
386
```
```
387
```					case 3:
```
388
```					//do nothing
```
389
```					break;
```
351 390
```				}
```
352
```
```
353
```
```
354
```				// COLOR afer DONE ---> MAGENTA
```
355
```				orb_set_color(MAGENTA);
```
356
```//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
```
357
```				correctTurn();			// turn to face the beacon
```
358
```				forward(220);
```
359
```				//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
```					//correctApproach();
```
368
```					distance = get_distance();
```
369
```					delay_ms(14);
```
370
```				}
```
371

372
```				stop();
```
373
```				orb_set_color(LIME);
```
374
```
```
375
```				send_buffer[0]=CIRCLE_ACTION_ACK;
```
391
```				/*send_buffer[0]=CIRCLE_ACTION_ACK;
```
376 392
```				send_buffer[1]=robotid;
```
377 393
```				wl_basic_send_global_packet(42,send_buffer,2);
```
378 394
```
```
......
406 422
```								/*usb_puts("bom_get_max : ");
```
407 423
```							usb_puti(max_bom);
```
408 424
```							usb_puts("/n/r");*/
```
425
```					/*
```
409 426
```
```
410
```
```
411 427
```							if(max_bom == 8)
```
412 428
```							{
```
413 429
```								orb2_set_color(BLUE);
```
......
434 450
```				delay_ms(10);
```
435 451
```
```
436 452
```				} //end while
```
437
```				break;
```
453
```				*/break;
```
438 454
```
```
439 455
```				// END for EDGE robots
```
440 456
```
```
......
449 465
```			case 1:			// BEACON /master /enter robots
```
450 466
```				switch(beacon_State) {
```
451 467
```				/*
```
452
```				   1. center  robots on wait for pressing button 1
```
468
```				   0. center  robots on wait for pressing button 1
```
453 469
```				*/
```
454 470
```				case 0:
```
455 471
```					bom_on();
```
......
467 483
```					beacon_State=2;
```
468 484
```					break;
```
469 485
```				/*
```
470
```					1. Count the number of the EDGE robots
```
486
```					2. Count the number of the EDGE robots
```
471 487
```					*******NOTE: at most  1500  times of loop ******
```
472 488
```				*/
```
473 489
```				case 2:
```
......
485 501
```							used[packet_data[1]]=1;
```
486 502
```						}
```
487 503
```					}
```
488
```					if(waitingCounter >= 1500){
```
504
```					if(waitingCounter >= 750){
```
489 505
```						beacon_State=3;
```
490 506
```					}
```
491 507
```					break;
```
......
504 520
```					Wait for all the robots to get to right distance/position
```
505 521
```				*/
```
506 522
```				case 4:
```
507
```					orb1_set_color(YELLOW);
```
523
```					/*orb1_set_color(YELLOW);
```
508 524
```					orb2_set_color(WHITE);
```
509 525
```
```
510 526
```					int numOk = 0;
```
......
518 534
```						}
```
519 535
```					}
```
520 536
```
```
521
```					beacon_State = 5;
```
537
```					beacon_State = 5;*/
```
522 538
```					break; // <----------------------------------------------------------Echo wrote this "break". need to be checked.
```
523 539
```
```
524 540
```				/**
```
525 541
```				    NEED to be documented
```
526 542
```				*/
```
527 543
```				case 5:
```
528
```					orb_set_color(BLUE);
```
544
```					/*orb_set_color(BLUE);
```
529 545
```					// clock for switching the BOMs between the master and slave
```
530 546
```					if(sending_counter++>4)
```
531 547
```					{
```
......
562 578
```						else bom_max_counter = 0;
```
563 579
```
```
564 580
```					}
```
565
```
```
581
```					*/
```
566 582
```					break;
```
567 583
```			}
```
568 584
```		}
```

Also available in: Unified diff