Project

General

Profile

Revision 1979

Added old files that used to compile,
new main in sendGraph that sends a graph packet - temporary

View differences:

trunk/code/projects/traffic_navigation/main-new.cold
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
trunk/code/projects/traffic_navigation/sendGraph.c
1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include "sendGraph.h"
4

  
5
int main(void){
6

  
7
    dragonfly_init(ALL_ON);
8
    wl_basic_init_default();
9
    wl_set_channel(12);
10

  
11
	node a;
12
	node b;
13
			
14
	edge aTob;
15
	edge bToa;
16
	
17
	//Create the edges
18
	aTob.to = 'b';
19
	aTob.dist = 25;
20
	
21
	bToa.to = 'a'; 
22
	bToa.dist = 25;
23
	
24
	
25
	a.outgoingEdges[0] = aTob;
26
	b.outgoingEdges[0] = bToa;
27
	
28
	
29
	a.type = 1;
30
	a.intNum = 1;
31
	a.numOut = 1;
32
	a.outSeen = 1;
33

  
34
	
35
	b.type = 1;
36
	b.intNum = 2;
37
	b.numOut = 1;
38
	b.outSeen = 1;
39

  
40
	intersections[0] = a;
41
	intersections[1] = b;
42
	
43
	sendIntersectionGraph();
44
}
45

  
46

  
47

  
48

  
49

  
50
/*
51
 * Uses wireless to send the graph sructure
52
 * to other robots
53
 *
54
 * @return 
55
 *
56
 */
57
void sendIntersectionGraph() {
58
        int i;
59
        node_union graph;
60

  
61
        for(i=0; i< NUM_FEATURES; i++){
62
                graph.n = intersections[i];
63
                wl_basic_send_global_packet(42, graph.array, 12);
64
                wl_basic_send_global_packet(42, '\n', 1);
65
        }
66
}
trunk/code/projects/traffic_navigation/sendGraph.h
1
#include "intersectData.h"
2

  
3
/* Data is sent in the following order
4
 * INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n
5
 * INT1_NUM OUTEDGE_0 ... \n
6
 * ...
7
 * ...
8
 */
9
void sendIntersectionGraph();
10

  
11

  
12
/* Representation of an edge (road) */
13
typedef struct {
14
	char to;	/* Where does this edge lead to? */
15
	char dist;	/* Where does it come from?	*/
16
}edge;
17

  
18
/* Representation of an intersection on the graph */
19
typedef struct {
20
	char type; 	/* Note that there are at most 5 intersection types */
21
	char intNum;	/* What is the intersection number */
22
	char numOut;	/* Note that we can have no more than 4 outgoing edges */
23
	char outSeen;	/* The number of the outgoing edges that we have seen for this intersection */
24
	edge outgoingEdges [4];  
25
}node;
26

  
27
/* A union that is used in wireless transmissions */
28
typedef union  {
29
	node n;
30
	char array[12];
31
}node_union;
32

  
33
/* This array holds all of the intersections that are represented in the graph
34
 * after its creation, the graph is transmitted wirelessly */
35
node intersections [NUM_FEATURES]; 
trunk/code/projects/traffic_navigation/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 = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/tty.usbserial*'; fi)
15 15

  
16 16
else
17 17
COLONYROOT := ../$(COLONYROOT)
trunk/code/projects/traffic_navigation/mapping.hold
1
#include "intersectData.h"
2

  
3
/* Data is sent in the following order
4
 * INT0_NUM OUTEDGE_0 OUTEDGE_1 ... \n
5
 * INT1_NUM OUTEDGE_0 ... \n
6
 * ...
7
 * ...
8
 */
9

  
10
/* Representation of an edge (road) */
11
typedef struct {
12
	char to;	/* Where does this edge lead to? */
13
	char dist;	/* Where does it come from?	*/
14
}edge
15

  
16
/* Representation of an intersection on the graph */
17
typedef struct {
18
	char type; 	/* Note that there are at most 5 intersection types */
19
	char intNum;	/* What is the intersection number */
20
	char numOut;	/* Note that we can have no more than 4 outgoing edges */
21
	char outSeen;	/* The number of the outgoing edges that we have seen for this intersection */
22
	edge[4] outgoingEdges;  
23
}node
24

  
25
/* A union that is used in wireless transmissions */
26
typedef union  {
27
	node n;
28
	char array[12];
29
}node_union
30

  
31
/* This array holds all of the intersections that are represented in the graph
32
 * after its creation, the graph is transmitted wirelessly */
33
node[NUM_FEATURES] intersections; 
34

  
35
int createEdge(edge* newEdge);
trunk/code/projects/traffic_navigation/main.cold
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
#ifndef MAIN_NEW
11
#ifndef WIRELESSWATCH
12

  
13
	static int state, sign, dataLength, pingWaitTime, turnDir;
14
	static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
15
	static unsigned char *packet;
16

  
17
	void enterIntersectionQueue(int sign);
18
	void waitInIntersectionQueue(void);
19
	void driveThroughIntersection(int sign);
20
	
21
int main (void) {
22
	
23
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
24
	dragonfly_init(ALL_ON);
25
	xbee_init();
26
	encoders_init();
27
	lineDrive_init();
28
	rtc_init(SIXTEENTH_SECOND, NULL);	
29
	wl_basic_init_default();
30
	wl_set_channel(13);
31
	initializeData();
32
	
33
	id = get_robotid();
34
	sign = 0;
35
	
36
	//Test code
37
	state = SROAD;
38

  
39
	sendBuffer[1] = id;
40

  
41
	/*
42
	doDrive(200);
43
	turn(DOUBLE, ILEFT);
44
	*/
45
/*	while(1)
46
		doDrive(255);
47
	*/
48
	
49
	while (1) {
50
		/*DTM Finite State Machine*/
51
		switch(state){
52
		case SROAD:/*Following a normal road*/
53
//			sign = lineFollow();
54
			//other road behaviors
55
				//tailgating?
56
			//read barcode
57
			sign = doDrive(200);
58
			if(button1_click()){
59
				state = SINTERSECTION;
60
			}
61
			break;
62
		case SINTERSECTION:/*Entering, and in intersection*/
63
			intersectionNum = 0;
64
			/*Intersection queue:
65
			 *Each robot when entering the intersection will check for other robots
66
			 *in the intersection, and insert itself in a queue to go through.
67
			 */
68
			prevBot = 0;
69
			nextBot = 0;
70
			
71
			sign = 0; //Test code until barcodes integrated
72
			
73
			enterIntersectionQueue(sign);
74
			
75
			orb1_set_color(PURPLE);
76
			//waits for its turn
77
			
78
			waitInIntersectionQueue();
79
			
80
			orb1_set_color(RED);
81
			//Drives through intersection
82
			driveThroughIntersection(sign);
83
			
84
			//Exits intersection
85
			sendBuffer[0] = WINTERSECTIONEXIT;
86
			sendBuffer[2] = intersectionNum;//Intersection #
87
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
88
			orb1_set_color(ORANGE);
89
			while(1){
90
				stop();
91
				sign = doDrive(200);
92
				if(button1_click()){
93
					start();
94
					state = SHIGHWAY;
95
					break;
96
				}
97
				if(button2_click()){
98
					start();
99
					state = SROAD;
100
					break;
101
				}
102
			}
103
			break;
104
		case SHIGHWAY:/*On highway*/
105
			orb1_set_color(CYAN);
106
			while(!button1_click()){
107
				highwayFSM();
108
			}
109
			state = SINTERSECTION;
110
			break;
111
		default:
112
			usb_puts("I got stuck in an unknown state! My state is ");
113
			usb_puti(state);
114
		}
115
	}
116

  
117
}
118

  
119
void enterIntersectionQueue(int sign){
120
		
121
	//Choose turn direction
122
	intersectionNum = getIntersectType(sign);
123
	turnDir = validateTurn(sign, getTurnType(4));
124
	//Sends packet announcing its entry to the intersection
125
	sendBuffer[0] = WINTERSECTIONENTRY;
126
	sendBuffer[2] = intersectionNum;//Intersection #
127
	sendBuffer[3] = getIntersectPos(sign);
128
	sendBuffer[4] = turnDir;
129
	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
130
	orb1_set_color(BLUE);
131
	stop();
132
	doDrive(0);
133
	rtc_reset();
134
	while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
135
		dataLength = 0;
136
		packet = NULL;
137
		packet = wl_basic_do_default(&dataLength);
138
		if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
139
			if(packet[0] == WINTERSECTIONREPLY){
140
				if(packet[3] == id){//Reply for me
141
					prevBot = packet[1];
142
					orb2_set_color(GREEN);
143
					break;
144
				}else if(packet[1] != id){//Someone else got here first, try again
145
					sendBuffer[0] = WINTERSECTIONENTRY;
146
					sendBuffer[2] = intersectionNum;
147
					sendBuffer[3] = getIntersectPos(sign);
148
					sendBuffer[4] = turnDir;
149
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
150
					orb2_set_color(ORANGE);
151
					rtc_reset();
152
				}
153
			}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
154
				sendBuffer[0] = WINTERSECTIONREPLY;
155
				sendBuffer[2] = intersectionNum;
156
				sendBuffer[3] = packet[1];
157
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
158
				nextBot = packet[1];
159
				nextDir = packet[3];
160
				nextPath = packet[4];
161
				orb2_set_color(BLUE);
162
				//delay_ms(200);
163
			}
164
				delay_ms(0);
165
		}
166
	}
167
}
168

  
169
void waitInIntersectionQueue(){
170
	while(prevBot != 0){
171
		dataLength = 0;
172
		packet = NULL;
173
		packet = wl_basic_do_default(&dataLength);
174
		if(packet && dataLength==PACKET_LENGTH){
175
			if(packet[2] == intersectionNum){
176
				if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
177
					prevBot = 0;
178
					orb2_set_color(PURPLE);
179
				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
180
					sendBuffer[0] = WINTERSECTIONREPLY;
181
					sendBuffer[2] = intersectionNum;
182
					sendBuffer[3] = packet[1];
183
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
184
					nextBot = packet[1];
185
					nextDir = packet[3];
186
					nextPath = packet[4];
187
					orb2_set_color(BLUE);
188
				}
189
			}
190
/*
191
			if(ISPING(packet)){
192
				sendBuffer[0] = WPINGREPLY;
193
				sendBuffer[2] = packet[1];
194
				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
195
					nextBot = packet[1];
196
				}
197
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
198
			}
199
			if(packet[0]==WPINGREPLY && packet[2]==id){
200
				prevBot = packet[1];
201
				pingWaitTime = rtc_get();
202
			}
203
*/
204
		}
205
					
206
		if(prevBot && rtc_get() << 12 == 0){
207
			sendBuffer[0] = WPINGBOT;
208
			sendBuffer[2] = prevBot;
209
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
210
			pingWaitTime = rtc_get();
211
		}
212
		if(prevBot && pingWaitTime - rtc_get() > 4){
213
			sendBuffer[0] = WPINGBOT;
214
			if(pingWaitTime - rtc_get() > 8){
215
				sendBuffer[0] = WPINGQUEUE;
216
			}
217
			sendBuffer[2] = prevBot;
218
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
219
		}
220
/*				if(prevBot && pingWaitTime - rtc_get() > 12){
221
			prevBot = 0;
222
		}
223
*/
224
	}
225
}
226

  
227
void driveThroughIntersection(int sign){
228
	//Code to choose path through intersection goes in this while loop
229
	//But here's the code to handle wireless while in the intersection...
230
	start();
231
	turn(getIntersectType(0), turnDir);
232
	while(doDrive(200) != FINISHED){
233
		dataLength = 0;
234
		packet = NULL;
235
		packet = wl_basic_do_default(&dataLength);		
236
		if(dataLength==PACKET_LENGTH){
237
			if(packet[2]==intersectionNum/*Intersection Num*/){
238
				if(packet[0]==WINTERSECTIONEXIT){
239
					prevBot = 0;
240
					orb2_set_color(RED);
241
				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
242
					sendBuffer[0] = WINTERSECTIONREPLY;
243
					sendBuffer[2] = intersectionNum;//Intersection #
244
					sendBuffer[3] = packet[1];
245
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
246
					nextBot = packet[1];
247
					nextDir = packet[3];
248
					nextPath = packet[4];
249
					orb2_set_color(YELLOW);
250
				}
251
			}
252
			if(ISPING(packet)){
253
				sendBuffer[0] = WPINGREPLY;
254
				sendBuffer[2] = packet[1];
255
				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
256
					nextBot = packet[1];
257
				}
258
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
259
			}
260
		}
261
		if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
262
			/*if(bots paths won't collide){
263
				sendBuffer[0] = WINTERSECTIONGO;
264
				sendBuffer[2] = 0;//Intersection #
265
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
266
			*/
267
		}
268
	}
269
}
270
#endif
271
#endif
trunk/code/projects/traffic_navigation/mapping.cold
1
#include ""
2
#include "mapping.h"
3
#include "lineDrive.h"
4
#include <dragonfly_lib.h>
5

  
6
//The last intersection that we encountered
7
int lastInt;
8

  
9

  
10
/* 
11
 * Traverses the map using DFS 
12
 * Returns 0 if all of the intersections in the database were seen
13
 * 1 otherwise 
14
 */
15
int createEdge(edge* newEdge){
16
	char barcode;
17
	char time;
18
	rtc_init(SIXTEENTH_SECOND, NULL);
19
	rtc_reset();
20
	while(barcode = NOBARCODE){
21
		barcode = (char) doDrive(200);
22
	}
23
	time = rtc_get();
24
	newEdge.to = barcode;
25
	newEdge.dist = time;
26
	return 0;
27
}
28

  
29
int createMap(){
30

  
31
    int seenout_count = 0;			/* The number of intersections we have completely seen */
32
    //int hist_count = 0;			/* The size of our history stack */ 
33

  
34
    char[NUM_FEATURES] seen;		/* Have we seen this node? */
35
    char[NUM_FEATURES] seenout;		/* Have we seen the outoging edges of this node? */
36
    //char[NUM_FEATURES] history;		/* A stack representing the history */
37

  
38
    int i;
39
    int outEdges, currInt, chosenEdge;
40

  
41

  
42
    /* Initialize all of these arrays */
43
    for(i = 0; i<NUM_FEATURES; i++) {
44
            seen[i] = 0;
45
            seenout[i] = 0;
46
            history[i] = 0;
47
            intersections[i].type = 0;
48
            intersections[i].intNum = 0;
49
            intersections[i].numOut = 0;
50
            intersections[i].outSeen = 0;
51
            intersections[i].outgoingEdges[0].to = 0;
52
            intersections[i].outgoingEdges[0].dist = 0;
53
            intersections[i].outgoingEdges[1].to = 0;
54
            intersections[i].outgoingEdges[1].dist = 0;
55
            intersections[i].outgoingEdges[2].to = 0;
56
            intersections[i].outgoingEdges[2].dist = 0;
57
            intersections[i].outgoingEdges[3].to = 0;
58
            intersections[i].outgoingEdges[3].dist = 0;	
59
    }
60

  
61

  
62
        /* Drives to the nearest intersection */
63
        barcode = nextInt();
64

  
65
        while(seenout_count < NUM_FEATURES)
66
        {
67
                currInt = getIntersectionNum(barcode);
68
                seen[currInt] = 1;
69

  
70
                /* Get the number of outgoing edges */
71
                outEdges = getNumOut(getIntersectType(currInt));
72

  
73
                /* Randomly choose an outgoing edge that we have not seen to go down 
74
                 * if no such edge exists */
75
                chosenEdge = rand() % outEdges;
76

  
77
                /* We have not seen all the outgoing edges */
78
                if(!seenout[currInt]){
79
                        intersection[currInt].outSeen++;
80
                        /* We have finished seeing all outgoing edges of the intersection */
81
                        if(intersections[currInt].numOut == intersection[currInt].outSeen){
82
                                seenout[currInt] = 1;
83
                                seenout_count ++;
84
                        }
85
                }
86
                /* Traverses edge, stores information in struct */
87
                traverseEdge(chosenEdge, &(intersection[currInt].outgoingEdges[chosenEdge]));
88
        }
89

  
90
        /* We are done traversing send the graph to the robots */
91
        sendIntersectionGraph();
92

  
93

  
94
        return 0;
95
}
96

  
97
/* Given an intersection type, returns the number of outgoing edges */
98
int getNumOut(int type) {
99
    switch(type){
100
        case 0: return;
101
        case 1:	return; 
102
        case 2:	return; 
103
        case 3:	return;
104
        case 4:	return;
105
    }
106

  
107
    return -1;
108
}
109

  
110

  
111
/* Creates an edge in the graph */
112
int insertEdge(){
113

  
114
}
115

  
116

  
117
/* 
118
 * Drives to the next intersection and returns its ID
119
 * If we are at a dead end, returns -1
120
 */
121
int nextInt(){
122

  
123
}
124

  
125

  
126
/*
127
 * Given an intersection node returns an integer that
128
 * can be sent wirelessly
129
 *
130
 */
131
int encodeNode(node n){
132

  
133

  
134
}
135

  
136
/*
137
 * Uses wireless to send the graph sructure
138
 * to other robots
139
 *
140
 * @return 
141
 *
142
 */
143
void sendIntersectionGraph() {
144
        int i;
145
        node_union graph;
146

  
147
        for(i=0; i< NUM_FEATURES; i++){
148
                graph.n = intersctions[i];
149
                wl_basic_send_global(42, graph.array, 12);
150
        }
151
}

Also available in: Unified diff