Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / main-new.cold @ 1979

History | View | Annotate | Download (13.4 KB)

1
/*
2
 * main.c for Traffic Navigation
3
 * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
4
 *
5
 * Author: Colony Project, CMU Robotics Club
6
 */
7

    
8
#include "traffic_navigation.h"
9
#include "../linefollowing/lineDrive.h"
10
#ifdef MAIN_NEW
11

    
12
	static int state, sign, turnDir;
13
	static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;
14
	unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;
15
	bool done;
16

    
17
	int wirelessPacketHandle(int state);
18
	void enterIntersection(void);
19
	void sendResolv(bool override);
20
	unsigned char resolvRandomNumberGen();
21

    
22
int main (void) {
23
	
24
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
25
	dragonfly_init(ALL_ON);
26
	xbee_init();
27
	encoders_init();
28
	lineDrive_init();
29
	rtc_init(SIXTEENTH_SECOND, NULL);	
30
	wl_basic_init_default();
31
	wl_set_channel(13);
32
	initializeData();
33
	
34
	id = get_robotid();
35
	sign = 0;
36
	orb1_set_color(GREEN);
37
	delay_ms(1000);
38
	orb1_set_color(ORB_OFF);
39
	
40
	//Test code
41
	state = SROAD;
42

    
43
	sendBuffer[1] = id;
44

    
45
	/*
46
	doDrive(200);
47
	turn(DOUBLE, ILEFT);
48
	*/
49
	/*
50
	while(1)
51
		doDrive(255);
52
	*/
53
	
54
	while (1) {
55
		/*DTM Finite State Machine*/
56
		switch(state){
57
		case SROAD:/*Following a normal road*/
58
			/* implement other road behaviors?
59
			 *    -tailgating
60
			 */
61
#ifdef ORB_INTERSECTION
62
			orb1_set_color(WHITE);
63
			orb2_set_color(ORB_OFF);
64
#endif
65
			start();
66
			done = false;
67
			while(!done){
68
				sign = doDrive(200);
69
				switch(sign){
70
					case NORMAL:
71
					case FINISHED:
72
					case LOST:
73
					case ERROR:
74
						break;
75
					default:
76
						//we have a barcode!
77
						state = SINTERSECTION_ENTER;
78
						done = true;
79
						break;
80

    
81
				}
82
			}
83
			break;
84
		case SINTERSECTION_ENTER:/*Entering, and in intersection*/
85
			stop();
86
			doDrive(0);
87
#ifdef ORB_INTERSECTION
88
			orb1_set_color(RED);
89
			orb2_set_color(ORB_OFF);
90
#endif
91

    
92
#ifdef DEBUG_INTERSECTION
93
			usb_puts("STATE: SINTERSECTION_ENTER\n");
94
#endif
95

    
96
			/*Intersection queue:
97
			 *Each robot when entering the intersection will check for other robots
98
			 *in the intersection, and insert itself in a queue to go through.
99
			 */
100
			queuePrevBot = -1; //the bot that will drive before this bot
101
			queueNextBot = -1; //the bot that will drive after this bot
102
			resolvPrevBotID = -3; //in the case of a race, the bot that is
103
			                      //to enter the queue before this bot
104
			resolvPrevBotDraw = 0; //the random priority number that bot has
105
			resolvDraw = 0; //my random priority number
106
			
107
			intersectionNum = getIntersectNum(sign);
108
			if(intersectionNum == (char) -1){ //invalid
109
				state = SROAD;
110
				break;
111
			}
112
			turnDir = validateTurn(sign, getTurnType(sign));
113
			if(turnDir == (char) -1){ //invalid
114
				state = SROAD;
115
				break;
116
			}
117

    
118
			enterIntersection(); //sends wireless packet for entry
119
			state = SINTERSECTION_WAIT;
120

    
121
			rtc_reset(); //reset rtc for timeout wait for reply
122
			done = false;
123
			char retried = 0;
124
			while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue
125
				int ret = wirelessPacketHandle(SINTERSECTION_ENTER);
126
				if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier...
127
#ifdef ORB_INTERSECTION
128
					orb2_set_color(PURPLE);
129
#endif
130
					enterIntersection();
131
					retried = 1;
132
				}
133
				switch (ret) {
134
					case KPLACEDINQUEUE:
135
#ifdef ORB_INTERSECTION
136
						orb2_set_color(GREEN);
137
#endif
138
						done = true;
139
						break;
140
					case KFAILEDTOQUEUE:
141
#ifdef DEBUG_INTERSECTION
142
						usb_puts("Failed to queue\n");
143
#endif
144
#ifdef ORB_INTERSECTION
145
						orb2_set_color(RED);
146
#endif
147
						enterIntersection();
148
						rtc_reset();
149
						break;
150
					case KRESOLVINGENTER:
151

    
152
#ifdef ORB_INTERSECTION
153
						orb2_set_color(ORANGE);
154
#endif
155
						state = SINTERSECTION_ENTER_RESOLV;
156
						done = true;
157
						break;
158
				}
159
			}
160
			break;
161

    
162
		case SINTERSECTION_ENTER_RESOLV:
163
#ifdef ORB_INTERSECTION
164
			orb1_set_color(PURPLE);
165
			orb2_set_color(ORB_OFF);
166
#endif
167
#ifdef DEBUG_INTERSECTION
168
			usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n");
169
#endif
170

    
171
			rtc_reset();
172
			done = false;
173
			retried = 0;
174
			while(rtc_get() < 9 && !done){
175
				int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
176
				switch (ret) {
177
					case KRESOLVINGENTER:
178
#ifdef ORB_INTERSECTION
179
						orb2_set_color(YELLOW);
180
#endif
181
						break;
182
					case KPLACEDINQUEUE:
183
#ifdef ORB_INTERSECTION
184
						orb2_set_color(GREEN);
185
#endif
186
						done = true;
187
						break;
188
					case KFAILEDTOQUEUE:
189
						usb_puts("Failed to queue\n");
190
						orb2_set_color(RED);
191
						enterIntersection();
192
						rtc_reset();
193
						break;
194
				}
195
				//if resolvPrevBotID == -1, this indicates that
196
				//there was a prevbot before, but it has entered
197
				//the queue so it's our turn.
198
				if(!done && resolvPrevBotID == (char) -1 && !retried){
199
					enterIntersection();
200
					rtc_reset();
201
					retried = 1;
202
				//if resolvPrevBotID == -2, we have been
203
				//resolving but never have seen a bot with lower
204
				//priority than us. after the 6/16ths sec
205
				//timeout, assume we are first.
206
				} else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){
207
					//send a intersection reply to myself to
208
					//trigger other bots to enter queue.
209
					sendBuffer[0] = WINTERSECTIONREPLY;
210
					sendBuffer[2] = intersectionNum;
211
					sendBuffer[3] = id;
212
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
213

    
214
					done = true;
215
					break;
216
				}
217
			}
218
			state = SINTERSECTION_WAIT;
219
			break;
220
		case SINTERSECTION_WAIT:/*Waiting in intersection */
221
#ifdef ORB_INTERSECTION
222
			orb1_set_color(YELLOW);
223
			orb2_set_color(ORB_OFF);
224
#endif
225
#ifdef DEBUG_INTERSECTION
226
			usb_puts("STATE: SINTERSECTION_WAIT\n");
227
#endif
228

    
229
			while(queuePrevBot != (char) -1){
230
				int ret = wirelessPacketHandle(state);
231
				switch (ret){
232
					case KFIRSTINQUEUE:
233
#ifdef ORB_INTERSECTION
234
						orb2_set_color(GREEN);
235
#endif
236
						state = SINTERSECTION_DRIVE;
237
						break;
238
					case KREPLIEDTOENTER:
239
#ifdef ORB_INTERSECTION
240
						orb2_set_color(BLUE);
241
#endif
242
						break;
243
					case KRESOLVINGENTER:
244
#ifdef ORB_INTERSECTION
245
						orb2_set_color(ORANGE);
246
#endif
247
						break;
248
				}
249
			}
250
			state = SINTERSECTION_DRIVE;
251
			break;
252
			
253
		case SINTERSECTION_DRIVE:
254
#ifdef DEBUG_INTERSECTION
255
			usb_puts("STATE: SINTERSECTION_DRIVE\n");
256
#endif
257
#ifdef ORB_INTERSECTION
258
			orb1_set_color(GREEN);
259
			orb2_set_color(ORB_OFF);
260
#endif
261
			start();
262
			turn(getIntersectType(sign), turnDir);
263
			while(doDrive(200) != FINISHED){
264
			//while(!button2_click()){
265
				 int ret = wirelessPacketHandle(state);
266
				 switch (ret){
267
					 case KREPLIEDTOENTER:
268
#ifdef ORB_INTERSECTION
269
						orb2_set_color(BLUE);
270
#endif
271
						break;
272
					 case KRESOLVINGENTER:
273
#ifdef ORB_INTERSECTION
274
					 	orb2_set_color(ORANGE);
275
#endif
276
						break;
277
				 }
278
			 }
279
			
280
			//Exits intersection
281
#ifdef ORB_INTERSECTION
282
			orb1_set_color(WHITE);
283
#endif
284

    
285
			sendBuffer[0] = WINTERSECTIONEXIT;
286
			sendBuffer[2] = intersectionNum;//Intersection #
287
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
288

    
289
			//Exits intersection
290
			/*
291
			while(1){
292
				if(button1_click()){
293
					start();
294
					state = SHIGHWAY;
295
					break;
296
				}
297
				if(button2_click()){
298
					start();
299
					state = SROAD;
300
					break;
301
				}
302
			}*/
303
			state = SROAD;
304
			break;
305
		case SHIGHWAY:/*On highway*/
306
#ifdef ORB_INTERSECTION
307
			orb1_set_color(CYAN);
308
#endif
309
			while(!button1_click()){
310
				highwayFSM();
311
			}
312
			state = SINTERSECTION_ENTER;
313
			break;
314
		default:
315
			usb_puts("I got stuck in an unknown state! My state is ");
316
			usb_puti(state);
317
		}
318
	}
319

    
320
}
321

    
322
int wirelessPacketHandle(int state){
323
	int dataLength = 0;
324
	unsigned char *packet = NULL;
325
	packet = wl_basic_do_default(&dataLength);
326
	
327
	/* sanity check */
328
	if(dataLength == 0 || packet == NULL) //no packet
329
		return ENOPACKET;
330

    
331
	if(dataLength != PACKET_LENGTH)
332
		return EPACKETLEN;
333

    
334
	usb_puts("Recieved Wireless Packet: ");
335
	for(int i = 0; i < dataLength; i++){
336
		usb_puti(packet[i]);
337
		usb_putc(' ');
338
	}
339
	usb_putc('\n');
340

    
341
	switch (packet[0]){
342
		case WROADENTRY: //[type, bot, road]
343
			return ENOACTION;
344
			break;
345
		case WROADEXIT: //[type, bot, road]
346
			return ENOACTION;
347
			break;
348
		case WROADSTOP: //[type, bot, road]
349
			return ENOACTION;
350
			break;
351
		case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]
352
			if (packet[2] == intersectionNum){
353
				switch (state){
354
					case SINTERSECTION_ENTER:
355
						sendResolv(false);
356
						resolvPrevBotID = -2;
357
						return KRESOLVINGENTER;
358
						break;
359
					case SINTERSECTION_ENTER_RESOLV:
360
						return ENOACTION;
361
					case SINTERSECTION_WAIT:
362
					case SINTERSECTION_DRIVE:
363
						if(queueNextBot == (char) -1){
364
                                        		sendBuffer[0] = WINTERSECTIONREPLY;
365
                                        		sendBuffer[2] = intersectionNum;
366
                                        		sendBuffer[3] = packet[1];
367
                                        		wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
368
							queueNextBot = packet[1];
369
							return KREPLIEDTOENTER;
370
						}
371
						break;
372

    
373
				}
374
			}
375
			break;
376
		case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]
377
			if (packet[2] == intersectionNum){
378
				switch (state){
379
					case SINTERSECTION_ENTER:
380
					case SINTERSECTION_ENTER_RESOLV:
381
						if(packet[3] == id){ //Reply for me
382
							queuePrevBot = packet[1];
383
							return KPLACEDINQUEUE;
384
						} else {
385
							if(packet[3] == resolvPrevBotID)
386
								resolvPrevBotID = -1;
387
							return KFAILEDTOQUEUE;
388
						}
389
						break;
390
					default:
391
						return ENOACTION;
392
				}
393
			}
394
			break;
395
		case WINTERSECTIONEXIT: //[type, bot, intersection]
396
			if (packet[2] == intersectionNum){
397
				switch (state){
398
					case SINTERSECTION_WAIT:
399
						if(packet[1]==queuePrevBot){
400
							queuePrevBot=-1;
401
							return KFIRSTINQUEUE;
402
						}
403
				}
404
			}
405
			break;
406
		case WINTERSECTIONGO: //[type, bot, intersection]
407
			break;
408
		case WINTERSECTIONPOLICEENTRY: //[?]
409
			return ENOACTION;
410
			break;
411
		case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]
412
			//in the case robots draw the same number these will be
413
			//arbitrated using the sending robot's id, prefering
414
			//lower ids
415
			usb_puts("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");
416
			usb_puti((int) resolvPrevBotID);
417
			usb_putc('\n');
418
			if (packet[2] == intersectionNum){
419
				switch (state){
420
					case SINTERSECTION_ENTER:
421
					case SINTERSECTION_ENTER_RESOLV:
422
						if(resolvPrevBotID == (char) -3){
423
							usb_puts("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");
424
							sendResolv(false);
425
							resolvPrevBotID = -2;
426
						}
427
						if((unsigned char) packet[3] == resolvDraw && id > packet[1]){
428
							//other bot drew same number as me,
429
							//and i have a higher id, so it goes first.
430
							usb_puts("bot ");
431
							usb_puti(packet[1]);
432
							usb_puts(" Drew the same number as me and I have a higher id, so it goes first\n");
433
							resolvPrevBotID = packet[1];
434
						} else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){
435
							//other bot drew same number as the bot before me,
436
							//so if it has a higher id than the bot before me,
437
							//it is going to go in between that one and me.
438
							usb_puts("bot ");
439
							usb_puti(packet[1]);
440
							usb_puts(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n");
441
							resolvPrevBotID = packet[1];
442
						} else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){
443
							//found a bot that goes in between the bot before me
444
							//and me, so now it is before me.
445
							usb_puts("bot ");
446
							usb_puti(packet[1]);
447
							usb_puts(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n");
448
							resolvPrevBotDraw = packet[3];
449
							resolvPrevBotID = packet[1];
450
						}
451
						return KRESOLVINGENTER;
452
						break;
453
					case SINTERSECTION_WAIT:
454
					case SINTERSECTION_DRIVE:
455
						usb_puts("Trying to resolv in non resolv case...queueNextbot = "); usb_puti(queueNextBot); usb_puts("\n");
456
						if(queueNextBot == (char) -1){
457
							sendResolv(true);
458
						}
459
						return KRESOLVINGENTER;
460
						break;
461
				}
462
				
463
			}
464
			break;
465
		case WHIGHWAYENTRY: //[type, bot, highway]
466
			return ENOACTION;
467
			break;
468
		case WHIGHWAYREPLY: //[type, fromBot, highway, toBot]
469
			return ENOACTION;
470
			break;
471
		case WHIGHWAYEXIT: //[type, bot, highway]
472
			return ENOACTION;
473
			break;
474
		case WPINGGLOBAL: //[type, bot]
475
			return ENOACTION;
476
			break;
477
		case WPINGBOT: //[type, fromBot, toBot]
478
			return ENOACTION;
479
			break;
480
		case WPINGQUEUE: //[type, fromBot, toBot]
481
			return ENOACTION;
482
			break;
483
		case WPINGREPLY: //[type, fromBot, toBot]
484
			return ENOACTION;
485
			break;
486
		case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces
487
			return ENOACTION;
488
			break;
489
		default:
490
			return ENOACTION;
491
			break;
492
	}
493
}
494

    
495
unsigned char resolvRandomNumberGen(){
496
	if ((resolvSeed *= (rtc_get())%9) == 0){
497
		return resolvSeed + 1; //0 is a reseved priority value for the last
498
		                 //bot that is already in the queue.
499
	}
500
	return resolvSeed;
501
}
502
void sendResolv(bool override){
503
	if(!override)
504
		resolvDraw = resolvRandomNumberGen();
505
	else
506
		resolvDraw = 0;
507
	sendBuffer[0] = WINTERSECTIONRESOLVERACE;
508
	sendBuffer[2] = intersectionNum;
509
	sendBuffer[3] = resolvDraw;
510
	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
511
}
512
void enterIntersection(void){
513
	//Sends packet announcing its entry to the intersection
514
	sendBuffer[0] = WINTERSECTIONENTRY;
515
	sendBuffer[2] = intersectionNum;//Intersection #
516
	sendBuffer[3] = 0; //getIntersectPos(sign);
517
	sendBuffer[4] = turnDir;
518
	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
519
}
520
#endif