root / trunk / code / projects / traffic_navigation / main.c @ 1863
History | View | Annotate | Download (8.21 KB)
1 | 1843 | bwasserm | /*
|
---|---|---|---|
2 | * main.c for Traffic Navigation
|
||
3 | * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
|
||
4 | *
|
||
5 | 1846 | bwasserm | * Author: Benjamin Wasserman, Colony Project, CMU Robotics Club
|
6 | 1843 | bwasserm | */
|
7 | |||
8 | 16 | bcoltin | #include <dragonfly_lib.h> |
9 | 1843 | bwasserm | #include <wl_basic.h> |
10 | 1863 | bwasserm | #include "../linefollowing/lineDrive.h" |
11 | 16 | bcoltin | |
12 | 1846 | bwasserm | |
13 | 1843 | bwasserm | /*States*/
|
14 | #define SROAD 0 |
||
15 | #define SINTERSECTION 10 |
||
16 | #define SHIGHWAY 20 |
||
17 | 1419 | jsexton | |
18 | 1843 | bwasserm | /*Sign Codes
|
19 | * bitwise OR labels to create a barcode or read one
|
||
20 | * There should be macros to extract these probably
|
||
21 | * The bits will be stored in some variable (char or short)
|
||
22 | * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD
|
||
23 | * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION
|
||
24 | */
|
||
25 | #define CROAD 0x0 //0b |
||
26 | #define CINTERSECTION 0x1 //1b |
||
27 | #define CNORMALROAD 0x0 //00b |
||
28 | #define CHIGHWAYROAD 0x2 //10b |
||
29 | #define C4WAY 0x0 //00b |
||
30 | #define C3WAY 0x2 //10b |
||
31 | #define CNORTH 0x0 //0000b |
||
32 | #define CEAST 0x4 //0100b |
||
33 | #define CSOUTH 0x8 //1000b |
||
34 | #define CWEST 0x12 //1100b |
||
35 | 967 | alevkoy | |
36 | 1844 | bwasserm | /*Wireless Packet Types
|
37 | * The first byte of any wireless packet should be one of these types.
|
||
38 | * Each type will have its own structure
|
||
39 | * The second byte should be the id of the bot sending the packet
|
||
40 | * The third byte should be the number of the intersection or road that
|
||
41 | * the packet pertains to
|
||
42 | */
|
||
43 | #define PACKET_LENGTH 5 |
||
44 | #define WROADENTRY 0 //[type, bot, road] |
||
45 | #define WROADREPLY 1 //[type, fromBot, road, toBot] |
||
46 | #define WROADEXIT 2 //[type, bot, road] |
||
47 | #define WROADSTOP 3 //[type, bot, road] |
||
48 | #define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir] |
||
49 | #define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot] |
||
50 | #define WINTERSECTIONEXIT 12 //[type, bot, intersection] |
||
51 | #define WINTERSECTIONGO 13 //[type, bot, intersection] |
||
52 | 1859 | bwasserm | #define WINTERSECTIONPOLICEENTRY 14 |
53 | 1844 | bwasserm | #define WHIGHWAYENTRY 20 //[type, bot, highway] |
54 | #define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot] |
||
55 | #define WHIGHWAYEXIT 22 //[type, bot, highway] |
||
56 | 1859 | bwasserm | #define WPINGGLOBAL 30 //[type, bot] |
57 | #define WPINGBOT 31 //[type, fromBot, toBot] |
||
58 | 1863 | bwasserm | #define WPINGQUEUE 32 //[type, fromBot, toBot] |
59 | 1859 | bwasserm | #define WPINGREPLY 33 //[type, fromBot, toBot] |
60 | 967 | alevkoy | |
61 | 1863 | bwasserm | /*Macros
|
62 | */
|
||
63 | #define ISPING(p) ((p)[0]==WPINGGLOBAL || (p)[0]==WPINGBOT || (p)[0]==WPINGQUEUE) |
||
64 | |||
65 | 1861 | bwasserm | 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 | 1843 | bwasserm | int main (void) { |
87 | 1419 | jsexton | |
88 | 1863 | bwasserm | int state, sign, dataLength, pingWaitTime;
|
89 | 1859 | bwasserm | char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum;
|
90 | 1419 | jsexton | |
91 | 1843 | bwasserm | /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
|
92 | dragonfly_init(ALL_ON); |
||
93 | xbee_init(); |
||
94 | encoders_init(); |
||
95 | 1863 | bwasserm | lineDrive_init(); |
96 | 1844 | bwasserm | rtc_init(SIXTEENTH_SECOND, NULL);
|
97 | 1859 | bwasserm | wl_basic_init_default(); |
98 | wl_set_channel(13);
|
||
99 | |||
100 | 1844 | bwasserm | id = get_robotid(); |
101 | 1843 | bwasserm | sign = 0;
|
102 | |||
103 | 1859 | bwasserm | //Test code
|
104 | state = SROAD; |
||
105 | |||
106 | 1844 | bwasserm | sendBuffer[1] = id;
|
107 | |||
108 | 1843 | bwasserm | while (1) { |
109 | /*DTM Finite State Machine*/
|
||
110 | switch(state){
|
||
111 | case SROAD:/*Following a normal road*/ |
||
112 | 1859 | bwasserm | // sign = lineFollow();
|
113 | 1843 | bwasserm | //other road behaviors
|
114 | //tailgating?
|
||
115 | //read barcode
|
||
116 | 1863 | bwasserm | doDrive(255);
|
117 | 1859 | bwasserm | if((sign & CINTERSECTION) || button1_click()){
|
118 | 1843 | bwasserm | state = SINTERSECTION; |
119 | } |
||
120 | break;
|
||
121 | case SINTERSECTION:/*Entering, and in intersection*/ |
||
122 | //Intersection behaviors
|
||
123 | //queueing
|
||
124 | //no-stop?
|
||
125 | //check wireless
|
||
126 | 1844 | bwasserm | |
127 | 1859 | bwasserm | intersectionNum = 0;
|
128 | |||
129 | 1844 | bwasserm | /*Intersection queue:
|
130 | *Each robot when entering the intersection will check for other robots
|
||
131 | *in the intersection, and insert itself in a queue to go through.
|
||
132 | */
|
||
133 | prevBot = 0;
|
||
134 | nextBot = 0;
|
||
135 | //Sends packet announcing its entry to the intersection
|
||
136 | sendBuffer[0] = WINTERSECTIONENTRY;
|
||
137 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
138 | 1844 | bwasserm | sendBuffer[3] = 0;//From Direction |
139 | 1859 | bwasserm | sendBuffer[4] = 2;//To Direction |
140 | 1844 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
141 | 1859 | bwasserm | orb1_set_color(BLUE); |
142 | 1863 | bwasserm | stop(); |
143 | 1844 | bwasserm | rtc_reset(); |
144 | while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue |
||
145 | packet = wl_basic_do_default(&dataLength); |
||
146 | 1859 | bwasserm | if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){ |
147 | 1844 | bwasserm | if(packet[3]==id){//Reply for me |
148 | prevBot = packet[1];
|
||
149 | 1859 | bwasserm | orb2_set_color(GREEN); |
150 | 1844 | bwasserm | break;
|
151 | }else{//Someone else got here first, try again |
||
152 | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
||
153 | rtc_reset(); |
||
154 | } |
||
155 | } |
||
156 | } |
||
157 | 1859 | bwasserm | orb1_set_color(PURPLE); |
158 | 1844 | bwasserm | //waits for its turn
|
159 | while(prevBot != 0){ |
||
160 | packet = wl_basic_do_default(&dataLength); |
||
161 | 1863 | bwasserm | 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 | } |
||
183 | 1844 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
184 | } |
||
185 | 1863 | bwasserm | if(packet[0]==WPINGREPLY && packet[2]==id){ |
186 | prevBot = packet[1];
|
||
187 | pingWaitTime = rtc_get(); |
||
188 | } |
||
189 | 1844 | bwasserm | } |
190 | 1863 | bwasserm | |
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(); |
||
196 | 1861 | bwasserm | } |
197 | 1863 | bwasserm | if(pingWaitTime - rtc_get() > 4){ |
198 | sendBuffer[0] = WPINGBOT;
|
||
199 | if(pingWaitTime - rtc_get() > 8){ |
||
200 | sendBuffer[0] = WPINGQUEUE;
|
||
201 | } |
||
202 | sendBuffer[2] = prevBot;
|
||
203 | 1861 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
204 | } |
||
205 | 1863 | bwasserm | if(pingWaitTime - rtc_get() > 12){ |
206 | prevBot = 0;
|
||
207 | } |
||
208 | 1844 | bwasserm | } |
209 | 1859 | bwasserm | orb1_set_color(RED); |
210 | 1844 | bwasserm | //Drives through intersection
|
211 | //Code to choose path through intersection goes in this while loop
|
||
212 | //But here's the code to handle wireless while in the intersection...
|
||
213 | 1863 | bwasserm | start(); |
214 | turn(DOUBLE, ILEFT);//Change paramers to variables
|
||
215 | 1859 | bwasserm | while(!button1_click()/*replace with variable to keep track of if in intersection*/){ |
216 | 1863 | bwasserm | doDrive(255);
|
217 | 1844 | bwasserm | packet = wl_basic_do_default(&dataLength); |
218 | 1863 | bwasserm | 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 | 1844 | bwasserm | 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 | //Exits intersection
|
||
252 | sendBuffer[0] = WINTERSECTIONEXIT;
|
||
253 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
254 | 1844 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
255 | 1859 | bwasserm | orb1_set_color(ORANGE); |
256 | 1844 | bwasserm | |
257 | 1843 | bwasserm | if(!(sign & CINTERSECTION)){
|
258 | if(sign & CHIGHWAYROAD){
|
||
259 | state = SHIGHWAY; |
||
260 | }else{
|
||
261 | state = SROAD; |
||
262 | } |
||
263 | } |
||
264 | break;
|
||
265 | case SHIGHWAY:/*On highway*/ |
||
266 | 1859 | bwasserm | // sign = lineFollow();
|
267 | 1843 | bwasserm | //highway behaviors
|
268 | //merging
|
||
269 | //passing
|
||
270 | //read barcode
|
||
271 | break;
|
||
272 | default:
|
||
273 | usb_puts("I got stuck in an unknown state! My state is ");
|
||
274 | usb_puti(state); |
||
275 | } |
||
276 | 967 | alevkoy | } |
277 | |||
278 | 16 | bcoltin | } |