Revision 1863
Completed adding ping capabilities for intersections. Code compiles, but untested on robots. Bots will ping each other in an intersection, and adapt if a bot disappears from the queue. Can only accept 1 robot disappearing at a time, unless vanishing robots aren't adjacent in the queue.
trunk/code/projects/traffic_navigation/main.c | ||
---|---|---|
7 | 7 |
|
8 | 8 |
#include <dragonfly_lib.h> |
9 | 9 |
#include <wl_basic.h> |
10 |
#include "../linefollowing/lineFollow.h"
|
|
10 |
#include "../linefollowing/lineDrive.h"
|
|
11 | 11 |
|
12 | 12 |
|
13 | 13 |
/*States*/ |
... | ... | |
55 | 55 |
#define WHIGHWAYEXIT 22 //[type, bot, highway] |
56 | 56 |
#define WPINGGLOBAL 30 //[type, bot] |
57 | 57 |
#define WPINGBOT 31 //[type, fromBot, toBot] |
58 |
#define WPINGQUEUE 32 //[type, fromBot, queue, toBot]
|
|
58 |
#define WPINGQUEUE 32 //[type, fromBot, toBot] |
|
59 | 59 |
#define WPINGREPLY 33 //[type, fromBot, toBot] |
60 | 60 |
|
61 |
/*Macros |
|
62 |
*/ |
|
63 |
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE) |
|
64 |
|
|
61 | 65 |
void pingReply(char ping[PACKET_LENGTH], char sendBuffer[PACKET_LENGTH], char nextBot){ |
62 | 66 |
sendBuffer[0] = WPINGREPLY; |
63 | 67 |
switch(ping[0]){ |
... | ... | |
81 | 85 |
|
82 | 86 |
int main (void) { |
83 | 87 |
|
84 |
int state, sign, dataLength; |
|
88 |
int state, sign, dataLength, pingWaitTime;
|
|
85 | 89 |
char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum; |
86 | 90 |
|
87 | 91 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
88 | 92 |
dragonfly_init(ALL_ON); |
89 | 93 |
xbee_init(); |
90 | 94 |
encoders_init(); |
91 |
lineFollow_init();
|
|
95 |
lineDrive_init();
|
|
92 | 96 |
rtc_init(SIXTEENTH_SECOND, NULL); |
93 | 97 |
wl_basic_init_default(); |
94 | 98 |
wl_set_channel(13); |
... | ... | |
109 | 113 |
//other road behaviors |
110 | 114 |
//tailgating? |
111 | 115 |
//read barcode |
116 |
doDrive(255); |
|
112 | 117 |
if((sign & CINTERSECTION) || button1_click()){ |
113 | 118 |
state = SINTERSECTION; |
114 | 119 |
} |
... | ... | |
134 | 139 |
sendBuffer[4] = 2;//To Direction |
135 | 140 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
136 | 141 |
orb1_set_color(BLUE); |
142 |
stop(); |
|
137 | 143 |
rtc_reset(); |
138 | 144 |
while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue |
139 | 145 |
packet = wl_basic_do_default(&dataLength); |
... | ... | |
152 | 158 |
//waits for its turn |
153 | 159 |
while(prevBot != 0){ |
154 | 160 |
packet = wl_basic_do_default(&dataLength); |
155 |
if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){ |
|
156 |
if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){ |
|
157 |
prevBot = 0; |
|
158 |
orb2_set_color(PURPLE); |
|
159 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
160 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
161 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
162 |
sendBuffer[3] = packet[1]; |
|
161 |
if(dataLength==PACKET_LENGTH){ |
|
162 |
if(packet[2] == intersectionNum){ |
|
163 |
if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){ |
|
164 |
prevBot = 0; |
|
165 |
orb2_set_color(PURPLE); |
|
166 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
167 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
168 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
169 |
sendBuffer[3] = packet[1]; |
|
170 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
171 |
nextBot = packet[1]; |
|
172 |
nextDir = packet[3]; |
|
173 |
nextPath = packet[4]; |
|
174 |
orb2_set_color(BLUE); |
|
175 |
} |
|
176 |
} |
|
177 |
if(ISPING(packet)){ |
|
178 |
sendBuffer[0] = WPINGREPLY; |
|
179 |
sendBuffer[2] = packet[1]; |
|
180 |
if(packet[0]==WPINGQUEUE && packet[3]==nextBot){ |
|
181 |
nextBot = packet[1]; |
|
182 |
} |
|
163 | 183 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
164 |
nextBot = packet[1]; |
|
165 |
nextDir = packet[3]; |
|
166 |
nextPath = packet[4]; |
|
167 |
orb2_set_color(BLUE); |
|
168 | 184 |
} |
185 |
if(packet[0]==WPINGREPLY && packet[2]==id){ |
|
186 |
prevBot = packet[1]; |
|
187 |
pingWaitTime = rtc_get(); |
|
188 |
} |
|
169 | 189 |
} |
170 |
if(dataLength==PACKET_LENGTH && (packet[0]==WPINGBOT || packet[0]==WPINGQUEUE)){ |
|
171 |
pingReply(packet, sendBuffer, nextBot); |
|
190 |
|
|
191 |
if(rtc_get() << 12 == 0){ |
|
192 |
sendBuffer[0] = WPINGBOT; |
|
193 |
sendBuffer[2] = prevBot; |
|
194 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
195 |
pingWaitTime = rtc_get(); |
|
172 | 196 |
} |
173 |
if(rtc_get()<<12 == 0){ |
|
174 |
sendBuffer[0] = WPINGQUEUE; |
|
175 |
sendBuffer[2] = intersectionNum; |
|
197 |
if(pingWaitTime - rtc_get() > 4){ |
|
198 |
sendBuffer[0] = WPINGBOT; |
|
199 |
if(pingWaitTime - rtc_get() > 8){ |
|
200 |
sendBuffer[0] = WPINGQUEUE; |
|
201 |
} |
|
202 |
sendBuffer[2] = prevBot; |
|
176 | 203 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
177 | 204 |
} |
205 |
if(pingWaitTime - rtc_get() > 12){ |
|
206 |
prevBot = 0; |
|
207 |
} |
|
178 | 208 |
} |
179 | 209 |
orb1_set_color(RED); |
180 | 210 |
//Drives through intersection |
181 | 211 |
//Code to choose path through intersection goes in this while loop |
182 | 212 |
//But here's the code to handle wireless while in the intersection... |
213 |
start(); |
|
214 |
turn(DOUBLE, ILEFT);//Change paramers to variables |
|
183 | 215 |
while(!button1_click()/*replace with variable to keep track of if in intersection*/){ |
216 |
doDrive(255); |
|
184 | 217 |
packet = wl_basic_do_default(&dataLength); |
185 |
if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){ |
|
186 |
if(packet[0]==WINTERSECTIONEXIT){ |
|
187 |
prevBot = 0; |
|
188 |
orb2_set_color(RED); |
|
189 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
190 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
191 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
192 |
sendBuffer[3] = packet[1]; |
|
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 |
} |
|
193 | 240 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
194 |
nextBot = packet[1]; |
|
195 |
nextDir = packet[3]; |
|
196 |
nextPath = packet[4]; |
|
197 |
orb2_set_color(YELLOW); |
|
198 | 241 |
} |
199 | 242 |
} |
200 | 243 |
if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time |
trunk/code/projects/traffic_navigation/Makefile | ||
---|---|---|
17 | 17 |
COLONYROOT := ../$(COLONYROOT) |
18 | 18 |
endif |
19 | 19 |
|
20 |
include $(COLONYROOT)/Makefile |
|
20 |
include $(COLONYROOT)/Makefile |
Also available in: Unified diff