Project

General

Profile

Revision 1888

Edited intersection behavior to break it into separate functions, and moved the #defines around.

View differences:

trunk/code/projects/traffic_navigation/main.c
7 7

  
8 8
#include "traffic_navigation.h"
9 9

  
10
	static int state, sign, dataLength, pingWaitTime;
11
	static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
12
	static unsigned char *packet;
13

  
10 14
int main (void) {
11 15

  
12
	int state, sign, dataLength, pingWaitTime;
13
	char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
14
	unsigned char *packet;
15
	
16 16
	/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
17 17
	dragonfly_init(ALL_ON);
18 18
	xbee_init();
......
26 26
	sign = 0;
27 27
	
28 28
	//Test code
29
	state = SHIGHWAY;
29
	state = SROAD;
30 30

  
31 31
	sendBuffer[1] = id;
32 32

  
33
	doDrive(255);
34
	turn(DOUBLE, ILEFT);
35
	
36
	while(1)
37
		doDrive(255);
38
	
39
	
33 40
	while (1) {
34 41
		/*DTM Finite State Machine*/
35 42
		switch(state){
......
38 45
			//other road behaviors
39 46
				//tailgating?
40 47
			//read barcode
41
//			doDrive(255);
42
			if((sign & CINTERSECTION) || button1_click()){
48
			sign = doDrive(255);
49
			if((sign >= 0) || button1_click()){
43 50
				state = SINTERSECTION;
44 51
			}
45 52
			break;
46 53
		case SINTERSECTION:/*Entering, and in intersection*/
47
			//Intersection behaviors
48
				//queueing
49
				//no-stop?
50
			//check wireless
51
			
52 54
			intersectionNum = 0;
53

  
54 55
			/*Intersection queue:
55 56
			 *Each robot when entering the intersection will check for other robots
56 57
			 *in the intersection, and insert itself in a queue to go through.
57 58
			 */
58 59
			prevBot = 0;
59 60
			nextBot = 0;
60
			//Sends packet announcing its entry to the intersection
61
			sendBuffer[0] = WINTERSECTIONENTRY;
62
			sendBuffer[2] = intersectionNum;//Intersection #
63
			sendBuffer[3] = 0;//From Direction
64
			sendBuffer[4] = 2;//To Direction
65
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
66
			orb1_set_color(BLUE);
67
//			stop();
68
			rtc_reset();
69
			while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
70
				dataLength = 0;
71
				packet = wl_basic_do_default(&dataLength);
72
				if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
73
					if(packet[0] == WINTERSECTIONREPLY){
74
						if(packet[3] == id){//Reply for me
75
							prevBot = packet[1];
76
							orb2_set_color(GREEN);
77
							break;
78
						}else if(packet[1] != id){//Someone else got here first, try again
79
							sendBuffer[0] = WINTERSECTIONENTRY;
80
							sendBuffer[2] = intersectionNum;//Intersection #
81
							sendBuffer[3] = 0;//From Direction
82
							sendBuffer[4] = 2;//To Direction
83
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
84
							orb2_set_color(ORANGE);
85
							rtc_reset();
86
						}
87
					}else /*if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
88
						sendBuffer[0] = WINTERSECTIONREPLY;
89
						sendBuffer[2] = intersectionNum;
90
						sendBuffer[3] = packet[1];
91
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
92
						nextBot = packet[1];
93
						nextDir = packet[3];
94
						nextPath = packet[4];
95
						orb2_set_color(BLUE);
96
						delay_ms(200);
97
					}*/
98
						delay_ms(0);
99
				}
100
			}
61
			enterIntersectionQueue();
62
			
101 63
			orb1_set_color(PURPLE);
102 64
			//waits for its turn
103
			while(prevBot != 0){
104
				dataLength = 0;
105
				packet = wl_basic_do_default(&dataLength);
106
				if(dataLength==PACKET_LENGTH){
107
					if(packet[2] == intersectionNum){
108
						if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
109
							prevBot = 0;
110
							orb2_set_color(PURPLE);
111
						}/*else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
112
							sendBuffer[0] = WINTERSECTIONREPLY;
113
							sendBuffer[2] = intersectionNum;
114
							sendBuffer[3] = packet[1];
115
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
116
							nextBot = packet[1];
117
							nextDir = packet[3];
118
							nextPath = packet[4];
119
							orb2_set_color(BLUE);
120
						}*/
121
					}
122
/*					if(ISPING(packet)){
123
						sendBuffer[0] = WPINGREPLY;
124
						sendBuffer[2] = packet[1];
125
						if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
126
							nextBot = packet[1];
127
						}
128
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
129
					}
130
					if(packet[0]==WPINGREPLY && packet[2]==id){
131
						prevBot = packet[1];
132
						pingWaitTime = rtc_get();
133
					}
134
*/				}
135
							
136
				if(prevBot && rtc_get() << 12 == 0){
137
					sendBuffer[0] = WPINGBOT;
138
					sendBuffer[2] = prevBot;
139
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
140
					pingWaitTime = rtc_get();
141
				}
142
				if(prevBot && pingWaitTime - rtc_get() > 4){
143
					sendBuffer[0] = WPINGBOT;
144
					if(pingWaitTime - rtc_get() > 8){
145
						sendBuffer[0] = WPINGQUEUE;
146
					}
147
					sendBuffer[2] = prevBot;
148
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
149
				}
150
/*				if(prevBot && pingWaitTime - rtc_get() > 12){
151
					prevBot = 0;
152
				}
153
*/			}
65
			
66
			waitInIntersectionQueue();
67
			
154 68
			orb1_set_color(RED);
155 69
			//Drives through intersection
156
			//Code to choose path through intersection goes in this while loop
157
			//But here's the code to handle wireless while in the intersection...
158
//			start();
159
//			turn(DOUBLE, ILEFT);//Change paramers to variables
160
			while(!button1_click()/*replace with variable to keep track of if in intersection*/){
161
//				doDrive(255);
162
				packet = wl_basic_do_default(&dataLength);		
163
				if(dataLength==PACKET_LENGTH){
164
					if(packet[2]==intersectionNum/*Intersection Num*/){
165
						if(packet[0]==WINTERSECTIONEXIT){
166
							prevBot = 0;
167
							orb2_set_color(RED);
168
						}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
169
							sendBuffer[0] = WINTERSECTIONREPLY;
170
							sendBuffer[2] = intersectionNum;//Intersection #
171
							sendBuffer[3] = packet[1];
172
							wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
173
							nextBot = packet[1];
174
							nextDir = packet[3];
175
							nextPath = packet[4];
176
							orb2_set_color(YELLOW);
177
						}
178
					}
179
					if(ISPING(packet)){
180
						sendBuffer[0] = WPINGREPLY;
181
						sendBuffer[2] = packet[1];
182
						if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
183
							nextBot = packet[1];
184
						}
185
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
186
					}
187
				}
188
				if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
189
					/*if(bots paths won't collide){
190
						sendBuffer[0] = WINTERSECTIONGO;
191
						sendBuffer[2] = 0;//Intersection #
192
						wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
193
					*/
194
				}
195
			}
70
			driveThroughIntersection();
71
			
196 72
			//Exits intersection
197 73
			sendBuffer[0] = WINTERSECTIONEXIT;
198 74
			sendBuffer[2] = intersectionNum;//Intersection #
199 75
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
200 76
			orb1_set_color(ORANGE);
201
			
202
			if(!(sign & CINTERSECTION)){
203
				if(sign & CHIGHWAYROAD){
204
					state = SHIGHWAY;
205
				}else{
206
					state = SROAD;
77
			while(1){
78
				sign = doDrive(255);
79
				if((sign >= 0) || button1_click()){
80
					if(0){
81
						state = SHIGHWAY;
82
						break;
83
					}else{
84
						state = SROAD;
85
						break;
86
					}
207 87
				}
208 88
			}
209 89
			break;
210 90
		case SHIGHWAY:/*On highway*/
211
			highwayFSM();
91
//			sign = lineFollow();
92
			//highway behaviors
93
				//merging
94
				//passing
95
			//read barcode
96
			break;
212 97
		default:
213 98
			usb_puts("I got stuck in an unknown state! My state is ");
214 99
			usb_puti(state);
......
216 101
	}
217 102

  
218 103
}
104

  
105
void enterIntersectionQueue(){
106
	//Sends packet announcing its entry to the intersection
107
	sendBuffer[0] = WINTERSECTIONENTRY;
108
	sendBuffer[2] = intersectionNum;//Intersection #
109
	sendBuffer[3] = 0;//From Direction
110
	sendBuffer[4] = 2;//To Direction
111
	wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
112
	orb1_set_color(BLUE);
113
	stop();
114
	doDrive(0);
115
	rtc_reset();
116
	while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
117
		dataLength = 0;
118
		packet = NULL;
119
		packet = wl_basic_do_default(&dataLength);
120
		if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
121
			if(packet[0] == WINTERSECTIONREPLY){
122
				if(packet[3] == id){//Reply for me
123
					prevBot = packet[1];
124
					orb2_set_color(GREEN);
125
					break;
126
				}else if(packet[1] != id){//Someone else got here first, try again
127
					sendBuffer[0] = WINTERSECTIONENTRY;
128
					sendBuffer[2] = intersectionNum;//Intersection #
129
					sendBuffer[3] = 0;//From Direction
130
					sendBuffer[4] = 2;//To Direction
131
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
132
					orb2_set_color(ORANGE);
133
					rtc_reset();
134
				}
135
			}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
136
				sendBuffer[0] = WINTERSECTIONREPLY;
137
				sendBuffer[2] = intersectionNum;
138
				sendBuffer[3] = packet[1];
139
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
140
				nextBot = packet[1];
141
				nextDir = packet[3];
142
				nextPath = packet[4];
143
				orb2_set_color(BLUE);
144
				//delay_ms(200);
145
			}
146
				delay_ms(0);
147
		}
148
	}
149
}
150

  
151
void waitInIntersectionQueue(){
152
	while(prevBot != 0){
153
		dataLength = 0;
154
		packet = NULL;
155
		packet = wl_basic_do_default(&dataLength);
156
		if(packet && dataLength==PACKET_LENGTH){
157
			if(packet[2] == intersectionNum){
158
				if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
159
					prevBot = 0;
160
					orb2_set_color(PURPLE);
161
				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
162
					sendBuffer[0] = WINTERSECTIONREPLY;
163
					sendBuffer[2] = intersectionNum;
164
					sendBuffer[3] = packet[1];
165
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
166
					nextBot = packet[1];
167
					nextDir = packet[3];
168
					nextPath = packet[4];
169
					orb2_set_color(BLUE);
170
				}
171
			}
172
/*
173
			if(ISPING(packet)){
174
				sendBuffer[0] = WPINGREPLY;
175
				sendBuffer[2] = packet[1];
176
				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
177
					nextBot = packet[1];
178
				}
179
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
180
			}
181
			if(packet[0]==WPINGREPLY && packet[2]==id){
182
				prevBot = packet[1];
183
				pingWaitTime = rtc_get();
184
			}
185
*/
186
		}
187
					
188
		if(prevBot && rtc_get() << 12 == 0){
189
			sendBuffer[0] = WPINGBOT;
190
			sendBuffer[2] = prevBot;
191
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
192
			pingWaitTime = rtc_get();
193
		}
194
		if(prevBot && pingWaitTime - rtc_get() > 4){
195
			sendBuffer[0] = WPINGBOT;
196
			if(pingWaitTime - rtc_get() > 8){
197
				sendBuffer[0] = WPINGQUEUE;
198
			}
199
			sendBuffer[2] = prevBot;
200
			wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
201
		}
202
/*				if(prevBot && pingWaitTime - rtc_get() > 12){
203
			prevBot = 0;
204
		}
205
*/
206
	}
207
}
208

  
209
void driveThroughIntersection(){
210
	//Code to choose path through intersection goes in this while loop
211
	//But here's the code to handle wireless while in the intersection...
212
	start();
213
	turn(DOUBLE, IRIGHT);//Change paramers to variables
214
	while(doDrive(255) != FINISHED){
215
		dataLength = 0;
216
		packet = NULL;
217
		packet = wl_basic_do_default(&dataLength);		
218
		if(dataLength==PACKET_LENGTH){
219
			if(packet[2]==intersectionNum/*Intersection Num*/){
220
				if(packet[0]==WINTERSECTIONEXIT){
221
					prevBot = 0;
222
					orb2_set_color(RED);
223
				}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
224
					sendBuffer[0] = WINTERSECTIONREPLY;
225
					sendBuffer[2] = intersectionNum;//Intersection #
226
					sendBuffer[3] = packet[1];
227
					wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
228
					nextBot = packet[1];
229
					nextDir = packet[3];
230
					nextPath = packet[4];
231
					orb2_set_color(YELLOW);
232
				}
233
			}
234
			if(ISPING(packet)){
235
				sendBuffer[0] = WPINGREPLY;
236
				sendBuffer[2] = packet[1];
237
				if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
238
					nextBot = packet[1];
239
				}
240
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
241
			}
242
		}
243
		if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
244
			/*if(bots paths won't collide){
245
				sendBuffer[0] = WINTERSECTIONGO;
246
				sendBuffer[2] = 0;//Intersection #
247
				wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
248
			*/
249
		}
250
	}
251
}
trunk/code/projects/traffic_navigation/Makefile
1
# this is a local makefile
1
# this is a local makefile special for traffic navigation
2 2

  
3 3
# Relative path to the root directory (containing lib directory)
4 4
ifndef COLONYROOT

Also available in: Unified diff