Revision 1869
Added code for pinging in intersections, but it currently doesn't work so its commented out. Intersections should work. Unfortunately some of Alex's doxygen code might be gone. Sorry.
trunk/code/projects/traffic_navigation/main.c | ||
---|---|---|
2 | 2 |
* main.c for Traffic Navigation |
3 | 3 |
* Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG |
4 | 4 |
* |
5 |
* Author: Benjamin Wasserman, Colony Project, CMU Robotics Club */ |
|
5 |
* Author: Colony Project, CMU Robotics Club |
|
6 |
*/ |
|
6 | 7 |
|
7 | 8 |
#include <dragonfly_lib.h> |
8 | 9 |
#include <wl_basic.h> |
... | ... | |
62 | 63 |
*/ |
63 | 64 |
#define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE) |
64 | 65 |
|
65 |
void pingReply(char ping[PACKET_LENGTH], char sendBuffer[PACKET_LENGTH], char nextBot){ |
|
66 |
sendBuffer[0] = WPINGREPLY; |
|
67 |
switch(ping[0]){ |
|
68 |
case WPINGBOT: |
|
69 |
sendBuffer[2] = ping[1]; |
|
70 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
71 |
break; |
|
72 |
case WPINGQUEUE: |
|
73 |
sendBuffer[2] = ping[1]; |
|
74 |
if(ping[3] == get_robotid()){ |
|
75 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
76 |
} |
|
77 |
if(ping[3] == nextBot){ |
|
78 |
//If next bot is missing from queue, take its place |
|
79 |
} |
|
80 |
break; |
|
81 |
default: |
|
82 |
break; |
|
83 |
} |
|
84 |
} |
|
85 |
|
|
86 | 66 |
int main (void) { |
87 | 67 |
|
88 | 68 |
int state, sign, dataLength, pingWaitTime; |
89 |
char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum; |
|
69 |
char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum; |
|
70 |
unsigned char *packet; |
|
90 | 71 |
|
91 | 72 |
/* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */ |
92 | 73 |
dragonfly_init(ALL_ON); |
93 | 74 |
xbee_init(); |
94 | 75 |
encoders_init(); |
95 |
lineDrive_init(); |
|
76 |
// lineDrive_init();
|
|
96 | 77 |
rtc_init(SIXTEENTH_SECOND, NULL); |
97 | 78 |
wl_basic_init_default(); |
98 | 79 |
wl_set_channel(13); |
... | ... | |
113 | 94 |
//other road behaviors |
114 | 95 |
//tailgating? |
115 | 96 |
//read barcode |
116 |
doDrive(255); |
|
97 |
// doDrive(255);
|
|
117 | 98 |
if((sign & CINTERSECTION) || button1_click()){ |
118 | 99 |
state = SINTERSECTION; |
119 | 100 |
} |
... | ... | |
139 | 120 |
sendBuffer[4] = 2;//To Direction |
140 | 121 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
141 | 122 |
orb1_set_color(BLUE); |
142 |
stop(); |
|
123 |
// stop();
|
|
143 | 124 |
rtc_reset(); |
144 | 125 |
while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue |
126 |
dataLength = 0; |
|
145 | 127 |
packet = wl_basic_do_default(&dataLength); |
146 |
if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){ |
|
147 |
if(packet[3]==id){//Reply for me |
|
148 |
prevBot = packet[1]; |
|
149 |
orb2_set_color(GREEN); |
|
150 |
break; |
|
151 |
}else{//Someone else got here first, try again |
|
128 |
if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum){ |
|
129 |
if(packet[0] == WINTERSECTIONREPLY){ |
|
130 |
if(packet[3] == id){//Reply for me |
|
131 |
prevBot = packet[1]; |
|
132 |
orb2_set_color(GREEN); |
|
133 |
break; |
|
134 |
}else if(packet[1] != id){//Someone else got here first, try again |
|
135 |
sendBuffer[0] = WINTERSECTIONENTRY; |
|
136 |
sendBuffer[2] = intersectionNum;//Intersection # |
|
137 |
sendBuffer[3] = 0;//From Direction |
|
138 |
sendBuffer[4] = 2;//To Direction |
|
139 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
|
140 |
orb2_set_color(ORANGE); |
|
141 |
rtc_reset(); |
|
142 |
} |
|
143 |
}else /*if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){ |
|
144 |
sendBuffer[0] = WINTERSECTIONREPLY; |
|
145 |
sendBuffer[2] = intersectionNum; |
|
146 |
sendBuffer[3] = packet[1]; |
|
152 | 147 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
153 |
rtc_reset(); |
|
154 |
} |
|
148 |
nextBot = packet[1]; |
|
149 |
nextDir = packet[3]; |
|
150 |
nextPath = packet[4]; |
|
151 |
orb2_set_color(BLUE); |
|
152 |
delay_ms(200); |
|
153 |
}*/ |
|
154 |
delay_ms(0); |
|
155 | 155 |
} |
156 | 156 |
} |
157 | 157 |
orb1_set_color(PURPLE); |
158 | 158 |
//waits for its turn |
159 | 159 |
while(prevBot != 0){ |
160 |
packet = wl_basic_do_default(&dataLength); |
|
160 |
dataLength = 0; |
|
161 |
packet = wl_basic_do_default(&dataLength); |
|
161 | 162 |
if(dataLength==PACKET_LENGTH){ |
162 | 163 |
if(packet[2] == intersectionNum){ |
163 | 164 |
if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){ |
164 | 165 |
prevBot = 0; |
165 | 166 |
orb2_set_color(PURPLE); |
166 |
}else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
|
167 |
}/*else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
|
|
167 | 168 |
sendBuffer[0] = WINTERSECTIONREPLY; |
168 |
sendBuffer[2] = intersectionNum;//Intersection #
|
|
169 |
sendBuffer[2] = intersectionNum; |
|
169 | 170 |
sendBuffer[3] = packet[1]; |
170 | 171 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
171 | 172 |
nextBot = packet[1]; |
172 | 173 |
nextDir = packet[3]; |
173 | 174 |
nextPath = packet[4]; |
174 | 175 |
orb2_set_color(BLUE); |
175 |
} |
|
176 |
}*/
|
|
176 | 177 |
} |
177 |
if(ISPING(packet)){ |
|
178 |
/* if(ISPING(packet)){
|
|
178 | 179 |
sendBuffer[0] = WPINGREPLY; |
179 | 180 |
sendBuffer[2] = packet[1]; |
180 | 181 |
if(packet[0]==WPINGQUEUE && packet[3]==nextBot){ |
... | ... | |
186 | 187 |
prevBot = packet[1]; |
187 | 188 |
pingWaitTime = rtc_get(); |
188 | 189 |
} |
189 |
} |
|
190 |
*/ }
|
|
190 | 191 |
|
191 |
if(rtc_get() << 12 == 0){ |
|
192 |
if(prevBot && rtc_get() << 12 == 0){
|
|
192 | 193 |
sendBuffer[0] = WPINGBOT; |
193 | 194 |
sendBuffer[2] = prevBot; |
194 | 195 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
195 | 196 |
pingWaitTime = rtc_get(); |
196 | 197 |
} |
197 |
if(pingWaitTime - rtc_get() > 4){ |
|
198 |
if(prevBot && pingWaitTime - rtc_get() > 4){
|
|
198 | 199 |
sendBuffer[0] = WPINGBOT; |
199 | 200 |
if(pingWaitTime - rtc_get() > 8){ |
200 | 201 |
sendBuffer[0] = WPINGQUEUE; |
... | ... | |
202 | 203 |
sendBuffer[2] = prevBot; |
203 | 204 |
wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH); |
204 | 205 |
} |
205 |
if(pingWaitTime - rtc_get() > 12){
|
|
206 |
/* if(prevBot && pingWaitTime - rtc_get() > 12){
|
|
206 | 207 |
prevBot = 0; |
207 | 208 |
} |
208 |
} |
|
209 |
*/ }
|
|
209 | 210 |
orb1_set_color(RED); |
210 | 211 |
//Drives through intersection |
211 | 212 |
//Code to choose path through intersection goes in this while loop |
212 | 213 |
//But here's the code to handle wireless while in the intersection... |
213 |
start(); |
|
214 |
turn(DOUBLE, ILEFT);//Change paramers to variables |
|
214 |
// start();
|
|
215 |
// turn(DOUBLE, ILEFT);//Change paramers to variables
|
|
215 | 216 |
while(!button1_click()/*replace with variable to keep track of if in intersection*/){ |
216 |
doDrive(255); |
|
217 |
// doDrive(255);
|
|
217 | 218 |
packet = wl_basic_do_default(&dataLength); |
218 | 219 |
if(dataLength==PACKET_LENGTH){ |
219 | 220 |
if(packet[2]==intersectionNum/*Intersection Num*/){ |
Also available in: Unified diff