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

View differences:

circle.c
6 6
int EDGE = 0;
7 7
int BEACON = 1;
8 8
int timeout = 0;
9
int sending = 0;
10
int stop2 = 0;
11
struct vector slave_position;
12
int desired_max_bom;
13
int bom_max_counter;
9 14

  
10 15

  
16
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

  
11 51
/*
12 52
	This program is used to make robots target a center (leader) robot using the BOM,
13 53
	then drive toward it and stop at a certain distance away.
......
83 123
}
84 124

  
85 125

  
126

  
127
/*~~~~~~~~~~~~~ NEED document
128
*/
86 129
int correctTurn(void)
87 130
{
88 131
	int bomNum = 0;
......
91 134
	usb_puti(bomNum);
92 135
	if(bomNum == 4)
93 136
	{	timeout = 0;
94
		motor_l_set(1, 200);
95
		motor_r_set(1, 200);
96
		return 0;
137
		//motor_l_set(1, 200);
138
		//motor_r_set(1, 200);
139
		return 1; //<--------------------------------------------1 and 0 are switched
97 140
	}
98 141
	else
99 142
	{
......
116 159
			motor_r_set(FORWARD, 200);
117 160
		}
118 161
	}
119
	return 1;
162
	return 0;
120 163
}
121 164

  
165

  
166
/*~~~~~~~~~~~~~~ NEED document
167
*/
122 168
void correctApproach(void)
123 169
{
124 170
	int bomNum = 0;
......
147 193
	}
148 194
}
149 195

  
150
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
196

  
197
/*
198
  Start turning  until the front receive the MAx bom reading
199
*/
151 200
void turn_to_beacon(int max){
152 201
	if (max>-1 && max<16){
153 202
		int index = (max+12)%16;
......
159 208
	}
160 209
}
161 210

  
211

  
212
/*
213
	Start turning its fron to the MAx bom reading
214
	But it won't stop by itself.
215
*/
162 216
void turn_to_beacon2(int max){				// like the previous but no stop() call'
163 217

  
164 218

  
......
174 228
	}
175 229
}
176 230

  
231

  
232

  
233

  
234
/*
235
	Turn towards the MAX bom direction  
236
	until the front receive the MAx bom reading
237
*/
177 238
void orient(void){
178 239
	int max_index = -1;
179 240
	while (max_index!=4) {
......
184 245
		delay_ms(22);
185 246
	}
186 247
}
248

  
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
*/
187 256
void orient2(void){
188 257
	int max_index = -1;
189 258
	while (max_index!=4) {
......
217 286
		count++;
218 287
	}
219 288
}
220
void go_straight_onefoot(void){				// essentially, another name for the above.  Should be removed.
221
	go_straight();
222
}
223 289

  
290

  
291
/* 
292
    BLINK the given number times
293
*/
224 294
void blink(int num) {
225 295
	for(int i = 0; i<num; i++)
226 296
	{
......
234 304

  
235 305

  
236 306

  
307

  
308

  
237 309
int main(void)
238 310
{
239 311
	/* Initialize dragonfly board */
......
243 315
    	/* Set the XBee channel to your assigned channel */	/* Set the XBee channel to your assigned channel */
244 316
	wl_set_channel(24);
245 317

  
318
	int sending_counter = 0;
246 319
	int robotid = get_robotid();
247 320
	int used[16];
248 321
	for (int i=0; i<16; i++) used[i] = 0;
......
269 342
	
270 343
	while(1)
271 344
	{
345
		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.
272 365
		switch(state)
273 366
		{
274
			case 0:			// EDGE
367
			/**********
368
				if  EDGE /slave robots
369
			*/
370
			case 0:	
275 371
				
276
				bom_off();
277
				while(1)
372
				/*
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)   
278 379
				{
380
					bom_off();
279 381
					orb1_set_color(YELLOW);orb2_set_color(CYAN);
280 382
					packet_data=wl_basic_do_default(&data_length);
281 383
					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
......
288 390
					}
289 391
				}
290 392
				
291
				while(1)		// wait for the center bot to count all the robots and send a "done" packet
393
				/*
394
					1. Wait for DONE package 
395
					2. The counting process is DONE
396
				*/
397
				while(1)		
292 398
				{
293 399
					orb_set_color(YELLOW);orb2_set_color(PURPLE);
294 400
					packet_data=wl_basic_do_default(&data_length);
......
300 406
					}
301 407
				}
302 408
				
409
				
410
				// COLOR afer DONE ---> MAGENTA
303 411
				orb_set_color(MAGENTA);
304 412
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
305 413
				while(correctTurn()){				
306 414
				}
415
				forward(220);
307 416
				//range_init();
308 417
				
309 418
				
......
319 428

  
320 429
				stop();
321 430
				orb_set_color(LIME);
322
				//button1_wait ();			// code for lab1.
323
				//go_straight_onefoot();
324
				break;
325 431
				
432
				send_buffer[0]=CIRCLE_ACTION_ACK;
433
				send_buffer[1]=robotid;	
434
				wl_basic_send_global_packet(42,send_buffer,2);
326 435
				
436
				// NEW CODE FROM JOEL
327 437
				
438
				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
					}
328 449
				
329
			case 1:			// BEACON
450
					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
330 507
				switch(beacon_State) {
331
				case 0:		// wait for a button to be pressed, then go to state 1
508
				/*
509
				   1. center  robots on wait for pressing button 1
510
				*/
511
				case 0:		
332 512
					bom_on();
333 513
					orb_set_color(PURPLE);
334 514
					if(button1_click()) beacon_State=1;
335
				break;	
515
					break;	
516
				/*
517
					1. Send EXIST package to EDGE robots
518
				*/
336 519
				case 1:		// sends a global exist packet to see how many robots there are
337 520
					orb_set_color(RED);
338 521
					send_buffer[0]=CIRCLE_ACTION_EXIST;
339 522
					send_buffer[1]=get_robotid();
340 523
					wl_basic_send_global_packet(42,send_buffer,2);
341 524
					beacon_State=2;
342
				break;
343
				case 2: 	// find out how many robots there are
344
				waitingCounter++;
345
				orb1_set_color(YELLOW);
346
				orb2_set_color(BLUE);
347
				packet_data=wl_basic_do_default(&data_length);
348
				if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
349
				{
350
					orb_set_color(RED);orb2_set_color(BLUE);
351
					if(used[packet_data[1]]==0){		//only add to robots seen if you haven't gotten an ACK from this robot
352
						robotsReceived++;
353
						used[packet_data[1]]=1;
525
					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
						}
354 543
					}
355
				}
356
				if(waitingCounter >= 1500){
357
					beacon_State=3;
358
					//blink(robotsReceived);
359
				}
360
				break;
544
					if(waitingCounter >= 1500){
545
						beacon_State=3;
546
					}
547
					break;
548
				/*
549
					COUNTing is DONE.
550
					Sending DONE package.
551
				*/	
361 552
				case 3:
362
				orb_set_color(GREEN);
363
				send_buffer[0]=CIRCLE_ACTION_DONE;
364
				wl_basic_send_global_packet(42,send_buffer,2);
365
				break;
366
				}
367
			break;
553
					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
			}
368 623
		}
369 624
	}
370 625

  
371 626
	orb_set_color(RED);
372 627
	while(1); /* END HERE */
373 628

  
374
	return 0;
629
	//return 0;
375 630
}

Also available in: Unified diff