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).
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