Project

General

Profile

Revision 1844

Added beginning of wireless protocol for traffic navigation, and added code for handling intersections, with the exception of that code that involves navigating the intersection (so just the code that deals with wireless, the intersection queue (as I see it), and deciding when to go).

View differences:

trunk/code/projects/traffic_navigation/main.c
32 32
#define CSOUTH 0x8 //1000b
33 33
#define CWEST 0x12 //1100b
34 34

  
35
/*Wireless Packet Types
36
 * The first byte of any wireless packet should be one of these types.
37
 * Each type will have its own structure
38
 * The second byte should be the id of the bot sending the packet
39
 * The third byte should be the number of the intersection or road that
40
 *   the packet pertains to
41
 */
42
#define PACKET_LENGTH 5
43
#define WROADENTRY 0 //[type, bot, road]
44
#define WROADREPLY 1 //[type, fromBot, road, toBot]
45
#define WROADEXIT 2 //[type, bot, road]
46
#define WROADSTOP 3 //[type, bot, road]
47
#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir]
48
#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]
49
#define WINTERSECTIONEXIT 12 //[type, bot, intersection]
50
#define WINTERSECTIONGO 13 //[type, bot, intersection]
51
#define WHIGHWAYENTRY 20 //[type, bot, highway]
52
#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]
53
#define WHIGHWAYEXIT 22 //[type, bot, highway]
35 54

  
36 55
int main (void) {
37 56

  
38
	int state, sign;
57
	int state, sign, dataLength;
58
	char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath;
39 59

  
40 60
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
41 61
	dragonfly_init(ALL_ON);
42 62
	xbee_init();
43 63
	encoders_init();
44 64
	lineFollow_init();
45
	
65
	rtc_init(SIXTEENTH_SECOND, NULL);	
66

  
67
	id = get_robotid();
46 68
	sign = 0;
47 69
	
70
	sendBuffer[1] = id;
71

  
48 72
	while (1) {
49 73
		/*DTM Finite State Machine*/
50 74
		switch(state){
51 75
		case SROAD:/*Following a normal road*/
52
			linefollow();
76
			sign = lineFollow();
53 77
			//other road behaviors
54 78
				//tailgating?
55 79
			//read barcode
......
62 86
				//queueing
63 87
				//no-stop?
64 88
			//check wireless
65
			//read barcode
89
			
90
			/*Intersection queue:
91
			 *Each robot when entering the intersection will check for other robots
92
			 *in the intersection, and insert itself in a queue to go through.
93
			 */
94
			prevBot = 0;
95
			nextBot = 0;
96
			//Sends packet announcing its entry to the intersection
97
			sendBuffer[0] = WINTERSECTIONENTRY;
98
			sendBuffer[2] = 0;//Intersection #
99
			sendBuffer[3] = 0;//From Direction
100
			sendBuffer[4] = 0;//To Direction
101
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
102
			rtc_reset();
103
			while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
104
				packet = wl_basic_do_default(&dataLength);
105
				if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==0/*Intersection Num*/){
106
					if(packet[3]==id){//Reply for me
107
						prevBot = packet[1];
108
						break;
109
					}else{//Someone else got here first, try again
110
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
111
						rtc_reset();
112
					}
113
				}
114
			}
115
			//waits for its turn
116
			while(prevBot != 0){
117
				packet = wl_basic_do_default(&dataLength);		
118
				if(dataLength==PACKET_LENGTH && packet[2]==0/*Intersection Num*/){
119
					if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
120
						prevBot = 0;
121
					}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
122
						sendBuffer[0] = WINTERSECTIONREPLY;
123
						sendBuffer[2] = 0;//Intersection #
124
						sendBuffer[3] = packet[1];
125
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
126
						nextBot = packet[1];
127
						nextDir = packet[3];
128
						nextPath = packet[4];
129
					}
130
				}
131
			}
132
			//Drives through intersection
133
			//Code to choose path through intersection goes in this while loop
134
			//But here's the code to handle wireless while in the intersection...
135
			while(1/*replace with variable to keep track of if in intersection*/){
136
				packet = wl_basic_do_default(&dataLength);		
137
				if(dataLength==PACKET_LENGTH && packet[2]==0/*Intersection Num*/){
138
					if(packet[0]==WINTERSECTIONEXIT){
139
						prevBot = 0;
140
					}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
141
						sendBuffer[0] = WINTERSECTIONREPLY;
142
						sendBuffer[2] = 0;//Intersection #
143
						sendBuffer[3] = packet[1];
144
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
145
						nextBot = packet[1];
146
						nextDir = packet[3];
147
						nextPath = packet[4];
148
					}
149
				}
150
				if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
151
					/*if(bots paths won't collide){
152
						sendBuffer[0] = WINTERSECTIONGO;
153
						sendBuffer[2] = 0;//Intersection #
154
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
155
					*/
156
				}
157
			}
158
			//Exits intersection
159
			sendBuffer[0] = WINTERSECTIONEXIT;
160
			sendBuffer[2] = 0;//Intersection #
161
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
162
			
66 163
			if(!(sign & CINTERSECTION)){
67 164
				if(sign & CHIGHWAYROAD){
68 165
					state = SHIGHWAY;
......
72 169
			}
73 170
			break;
74 171
		case SHIGHWAY:/*On highway*/
75
			linefollow();
172
			sign = lineFollow();
76 173
			//highway behaviors
77 174
				//merging
78 175
				//passing

Also available in: Unified diff