Revision 1627

View differences:

trunk/code/behaviors/formation_control/circle/Makefile
11 11
USE_WIRELESS = 1
12 12

  
13 13
# com1 = serial port. Use lpt1 to connect to parallel port.
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
14
AVRDUDE_PORT = COM4
15
#AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi)
15 16
else
16 17
COLONYROOT := ../$(COLONYROOT)
17 18
endif
trunk/code/behaviors/formation_control/circle/circle.c
33 33
	if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15;
34 34
}
35 35

  
36
void switch_sending ()
36
void switch_sending(void)
37 37
{
38 38
	if(sending)
39 39
	{
......
126 126

  
127 127
/*~~~~~~~~~~~~~ NEED document
128 128
*/
129
int correctTurn(void)
129
void correctTurn(void)
130 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
		//motor_l_set(1, 200);
138
		//motor_r_set(1, 200);
139
		return 1; //<--------------------------------------------1 and 0 are switched
140
	}
141
	else
131
	orb1_set_color(BLUE);			// BLUE and PURPLE
132
	left(220);
133
	while(1)
142 134
	{
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))
135
		int bomNum = 0;				// bomNum is the current maximum reading
136
		bom_refresh(BOM_ALL);
137
		bomNum = bom_get_max();
138
		usb_puti(bomNum);
139
		if(bomNum == 4)				// when it's turned the right way, stop
152 140
		{
153
			motor_l_set(FORWARD, 200);timeout = 0;
154
			motor_r_set(BACKWARD, 200);
141
			timeout = 0;
142
			//motor_l_set(1, 200);
143
			//motor_r_set(1, 200);
144
			break;					// exits the while() loop to stop the method
155 145
		}
156
		else
146
		else						// facing the wrong way
157 147
		{
158
			motor_l_set(BACKWARD, 200);timeout = 0;
159
			motor_r_set(FORWARD, 200);
148
			if(bomNum == -1){
149
				
150
				timeout++;
151
				
152
				if(timeout > 500000)		// if it's been looking too long, move a little bit as it turns
153
				{
154
					motor_r_set(FORWARD, 210);
155
					motor_l_set(BACKWARD, 190);
156
				}
157
			}
158
			else if((bomNum >= 12) || (bomNum < 4))
159
			{
160
				motor_l_set(FORWARD, 200);
161
				motor_r_set(BACKWARD, 200);
162
				timeout = 0;
163
			}
164
			else
165
			{
166
				motor_l_set(BACKWARD, 200);
167
				motor_r_set(FORWARD, 200);
168
				timeout = 0;
169
			}
160 170
		}
161 171
	}
162
	return 0;
172
	return;
163 173
}
164 174

  
165 175

  
166 176
/*~~~~~~~~~~~~~~ NEED document
177

  
178
	Actually, we should just get rid of this.
167 179
*/
168 180
void correctApproach(void)
169 181
{
......
194 206
}
195 207

  
196 208

  
197
/*
198
  Start turning  until the front receive the MAx bom reading
199
*/
200
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

  
212
/*
213
	Start turning its fron to the MAx bom reading
214
	But it won't stop by itself.
215
*/
216
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

  
231

  
232

  
233

  
234
/*
235
	Turn towards the MAX bom direction  
236
	until the front receive the MAx bom reading
237
*/
238
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

  
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
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 209
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
267 210
void go_straight(void){						// drives forward a hardcoded distance. May not be useful.
268 211
	forward(200);
......
328 271
	int beacon_State=0;
329 272
	int waitingCounter=0;
330 273
	int robotsReceived=0;
274
	
331 275
	if(wheel()<100)
332 276
	{
333 277
		state=EDGE;
......
410 354
				// COLOR afer DONE ---> MAGENTA
411 355
				orb_set_color(MAGENTA);
412 356
//~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
413
				while(correctTurn()){				
414
				}
357
				correctTurn();			// turn to face the beacon
415 358
				forward(220);
416 359
				//range_init();
417 360
				
......
534 477
					packet_data=wl_basic_do_default(&data_length);
535 478
					if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
536 479
					{
537
						orb_set_color(RED);orb2_set_color(BLUE);
480
						orb_set_color(RED);
481
						orb2_set_color(BLUE);
538 482
						//only add to robots seen if you haven't gotten an ACK from this robot
539 483
						if(used[packet_data[1]]==0){		
540 484
							robotsReceived++;
......
550 494
					Sending DONE package.
551 495
				*/	
552 496
				case 3:
497
					blink(robotsReceived);
553 498
					orb_set_color(GREEN);
554 499
					send_buffer[0]=CIRCLE_ACTION_DONE;
555 500
					wl_basic_send_global_packet(42,send_buffer,2);

Also available in: Unified diff