Revision 1568

View differences:

trunk/code/behaviors/formation_control/circle2/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)
15

  
14
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM7:'; else echo '/dev/ttyUSB0'; fi)
16 15
else
17 16
COLONYROOT := ../$(COLONYROOT)
18 17
endif
trunk/code/behaviors/formation_control/circle2/circle.c
15 15
	With adjustment, the leader robot will be able to turn and use its standardized
16 16
	rangefinder to reposition or space the robots evenly.
17 17
	
18
	AuTHORS: Niko, Alex, Reva, Echo, Steve
18
	AuTHORS: Nico, Alex, Reva, Echo, Steve
19 19
*/
20 20

  
21 21

  
22 22
/* 
23 23
TODO:
24
	Check BOM more often and before final stop
25
	Make BOM check more comprehensively
26
	Use the center bot to check distances
24
		Check BOM more often and before final stop
25
		Make BOM check more comprehensively
26
		Use the center bot to check distances
27 27
		Count them ("spam" method)
28 28
		Use beacon to find relative positions
29 29
		Beacon tells them how to move to be at the right distance
30
	Wireless communication, initialization
30
		*done*Wireless communication, initialization
31 31
*/
32 32

  
33 33
void forward(int speed){			// set the motors to this forward speed.
......
61 61
		temp = range_read_distance(IR2);
62 62
		if (temp == -1)
63 63
		{
64
			temp=0;
65
			kk--;
64
			//temp=0;
65
			i--;
66 66
		}
67
		distance+= temp;
67
		else
68
			distance+= temp;
68 69
		delay_ms(3);
69 70
	}
70 71
	if (kk>0)
......
143 144
	for(int i = 0; i<num; i++)
144 145
	{
145 146
		orb_set_color(ORB_OFF);
146
		delay_ms(200);
147
		delay_ms(500);
147 148
		orb_set_color(RED);
148
		delay_ms(200);
149
		delay_ms(500);
149 150
	}
150 151
	orb_set_color(ORB_OFF);
151 152
}
......
161 162
	/* Initialize the basic wireless library */
162 163
	wl_basic_init_default();
163 164
	/* Set the XBee channel to your assigned channel */
164
	wl_set_channel(12);
165
	wl_set_channel(24);
165 166

  
166 167
	int robotid = get_robotid();
167 168
	char send_buffer[2];
......
170 171
	
171 172
	
172 173
	int state = EDGE;
174
	int beacon_State=0;
175
	int NUM_ROBOTS=2;
176
	int robotsRecieved=0;
173 177
	if(wheel()<100)
174 178
	{
175 179
		state=EDGE;
......
197 201
						send_buffer[1]=get_robotid();
198 202
						
199 203
						wl_basic_send_global_packet(42,send_buffer,2);
204
						break;
200 205
					}
201
					
202
					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
206
				}
207
				
208
				while(1){
209
					orb_set_color(RED);
210
					packet_data=wl_basic_do_default(&data_length);
211
					wl_basic_send_global_packet(42,send_buffer,2);
212

  
213
					if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
203 214
					{
204 215
						break;
205 216
					}
206 217
				}
218
				
207 219
				orb_set_color(GREEN);
208 220
				
209 221
				orient();
......
215 227
				distance = get_distance();
216 228
				while (distance>=onefoot || distance==0)
217 229
				{
230
					if(distance==0)
231
						orb_set_color(WHITE);
218 232
					distance = get_distance();
219 233
					orient2();
220 234
					forward(speed);
......
230 244
				
231 245
				
232 246
			case 1:			// BEACON
233
				bom_on();
234
				orb_set_color(PURPLE);
235
				
236
				int numrobots = 0;
237
				
238
				while(!button1_click())
239
				{ }
240
				
241
				send_buffer[0]=CIRCLE_ACTION_EXIST;
242
				send_buffer[1]=get_robotid();
243
				
244
				wl_basic_send_global_packet(42,send_buffer,2);
245
				
247
				switch(beacon_State) {
248
				case 0:
249
					bom_on();
250
					orb_set_color(PURPLE);
251
					if(button1_click()) beacon_State=1;
252
				break;	
253
				case 1:
254
					send_buffer[0]=CIRCLE_ACTION_EXIST;
255
					send_buffer[1]=get_robotid();
256
					wl_basic_send_global_packet(42,send_buffer,2);
257
					beacon_State=2;
258
				break;
259
				case 2: 
260
				orb_set_color(ORANGE);
246 261
				packet_data=wl_basic_do_default(&data_length);
247
				while(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
262
				if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
248 263
				{
249
					// IF NEEDED: a good place to collect and store the robot id.
250
					numrobots++;
264
					orb_set_color(WHITE);
265
					robotsRecieved++;
251 266
				}
252
				
253
				send_buffer[0]=CIRCLE_ACTION_ACK;
267
				if(robotsRecieved==NUM_ROBOTS)
268
					beacon_State=3;
269
				break;
270
				case 3:
271
				send_buffer[0]=CIRCLE_ACTION_DONE;
254 272
				wl_basic_send_global_packet(42,send_buffer,2);
255
				
256
				blink(numrobots);
257
				
258
				
273
				break;
274
				}
259 275
			break;
260 276
		}
261 277
	}
trunk/code/behaviors/formation_control/circle2/circle.h
14 14
#define CIRCLE_ACTION_EXIST 'E'
15 15
#define CIRCLE_ACTION_POSITION 'P'
16 16
#define CIRCLE_ACTION_ACK 'A'
17
#define CIRCLE_ACTION_DONE 'D'
17 18
//#define EDGE 0;
18 19
//#define BEACON 1;
19 20

  

Also available in: Unified diff