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

View differences:

circle.c
5 5

  
6 6
int EDGE = 0;
7 7
int BEACON = 1;
8
int timeout = 0;
8 9

  
10

  
9 11
/*
10 12
	This program is used to make robots target a center (leader) robot using the BOM,
11 13
	then drive toward it and stop at a certain distance away.
......
34 36
		*done*Wireless communication, initialization
35 37
*/
36 38

  
39

  
40

  
37 41
void forward(int speed){			// set the motors to this forward speed.
38 42
	motor_l_set(FORWARD,speed);
39 43
	motor_r_set(FORWARD,speed);
......
77 81
	else 
78 82
		return 0;
79 83
}
84

  
85

  
86
int correctTurn(void)
87
{
88
	int bomNum = 0;
89
	bom_refresh(BOM_ALL);
90
	bomNum = bom_get_max();
91
	usb_puti(bomNum);
92
	if(bomNum == 4)
93
	{	timeout = 0;
94
		motor_l_set(1, 200);
95
		motor_r_set(1, 200);
96
		return 0;
97
	}
98
	else
99
	{
100
		if(bomNum == -1){
101
			timeout++;
102
			if(timeout > 500000)
103
			{
104
				motor_r_set(FORWARD, 210);
105
				motor_l_set(BACKWARD, 190);
106
			}
107
		}
108
		else if((bomNum >= 12) || (bomNum < 4))
109
		{
110
			motor_l_set(FORWARD, 200);timeout = 0;
111
			motor_r_set(BACKWARD, 200);
112
		}
113
		else
114
		{
115
			motor_l_set(BACKWARD, 200);timeout = 0;
116
			motor_r_set(FORWARD, 200);
117
		}
118
	}
119
	return 1;
120
}
121

  
122
void correctApproach(void)
123
{
124
	int bomNum = 0;
125
	bom_refresh(BOM_ALL);
126
	bomNum = bom_get_max();
127
	usb_puti(bomNum);
128
	if(bomNum == 4)
129
	{	
130
		motor_l_set(1, 200);
131
		motor_r_set(1, 200);
132
	}
133
	else
134
	{
135
		if(bomNum == -1){}
136
		else if((bomNum >= 12) || (bomNum < 4))
137
		{
138
			motor_l_set(FORWARD, 200);
139
			motor_r_set(BACKWARD, 200);
140
		}
141
		else
142
		{
143
			motor_l_set(BACKWARD, 200);
144
			motor_r_set(FORWARD, 200);
145
		}
146
		delay_ms(100);
147
	}
148
}
149

  
150
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
80 151
void turn_to_beacon(int max){
81 152
	if (max>-1 && max<16){
82 153
		int index = (max+12)%16;
......
102 173
		else left(170);
103 174
	}
104 175
}
176

  
105 177
void orient(void){
106 178
	int max_index = -1;
107 179
	while (max_index!=4) {
......
122 194
		delay_ms(22);
123 195
	}
124 196
}
197
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
125 198
void go_straight(void){						// drives forward a hardcoded distance. May not be useful.
126 199
	forward(200);
127 200
	encoder_rst_dx(LEFT);
......
164 237
int main(void)
165 238
{
166 239
	/* Initialize dragonfly board */
167
	dragonfly_init(ALL_ON);
168
	xbee_init();
169
	encoders_init();
170
	/* Initialize the basic wireless library */
171
	wl_basic_init_default();
172
	/* Set the XBee channel to your assigned channel */
240
    	dragonfly_init(ALL_ON);
241
    	/* Initialize the basic wireless library */
242
    	wl_basic_init_default();
243
    	/* Set the XBee channel to your assigned channel */	/* Set the XBee channel to your assigned channel */
173 244
	wl_set_channel(24);
174 245

  
175 246
	int robotid = get_robotid();
......
202 273
		{
203 274
			case 0:			// EDGE
204 275
				
205
				orb_set_color(GREEN);
276
				bom_off();
206 277
				while(1)
207 278
				{
208
					orb_set_color(YELLOW);
279
					orb1_set_color(YELLOW);orb2_set_color(CYAN);
209 280
					packet_data=wl_basic_do_default(&data_length);
210 281
					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
211 282
					{
......
219 290
				
220 291
				while(1)		// wait for the center bot to count all the robots and send a "done" packet
221 292
				{
222
					orb_set_color(RED);
293
					orb_set_color(YELLOW);orb2_set_color(PURPLE);
223 294
					packet_data=wl_basic_do_default(&data_length);
224 295
					wl_basic_send_global_packet(42,send_buffer,2);
225 296
					
......
229 300
					}
230 301
				}
231 302
				
232
				orb_set_color(GREEN);
233
				
234
				orient();
235
				forward(speed);
303
				orb_set_color(MAGENTA);
304
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
305
				while(correctTurn()){				
306
				}
236 307
				//range_init();
237 308
				
238
				orb_set_color(BLUE);
239 309
				
240 310
				distance = get_distance();
241 311
				while (distance>=onefoot || distance==0)
242 312
				{
243 313
					if(distance==0)
244 314
						orb_set_color(WHITE);
315
					//correctApproach();
245 316
					distance = get_distance();
246
					orient2();
247
					forward(speed);
248 317
					delay_ms(14);
249 318
				}
319

  
250 320
				stop();
251
				orient();
252
				
321
				orb_set_color(LIME);
253 322
				//button1_wait ();			// code for lab1.
254 323
				//go_straight_onefoot();
255
				stop();
256 324
				break;
257 325
				
258 326
				
......
266 334
					if(button1_click()) beacon_State=1;
267 335
				break;	
268 336
				case 1:		// sends a global exist packet to see how many robots there are
337
					orb_set_color(RED);
269 338
					send_buffer[0]=CIRCLE_ACTION_EXIST;
270 339
					send_buffer[1]=get_robotid();
271 340
					wl_basic_send_global_packet(42,send_buffer,2);
......
273 342
				break;
274 343
				case 2: 	// find out how many robots there are
275 344
				waitingCounter++;
276
				orb_set_color(ORANGE);
345
				orb1_set_color(YELLOW);
346
				orb2_set_color(BLUE);
277 347
				packet_data=wl_basic_do_default(&data_length);
278 348
				if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
279 349
				{
280
					orb_set_color(WHITE);
350
					orb_set_color(RED);orb2_set_color(BLUE);
281 351
					if(used[packet_data[1]]==0){		//only add to robots seen if you haven't gotten an ACK from this robot
282 352
						robotsReceived++;
283 353
						used[packet_data[1]]=1;
......
289 359
				}
290 360
				break;
291 361
				case 3:
292
				orb_set_color(PURPLE);
362
				orb_set_color(GREEN);
293 363
				send_buffer[0]=CIRCLE_ACTION_DONE;
294 364
				wl_basic_send_global_packet(42,send_buffer,2);
295 365
				break;

Also available in: Unified diff