Revision 1635 trunk/code/behaviors/formation_control/circle/circle.c

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