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