root / trunk / code / projects / traffic_navigation / main.c @ 1860
History | View | Annotate | Download (6.33 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 | 1860 | djacobs | #include "../linefollowing/lineFollow.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 | #define WPINGQUEUE 32 //[type, bot, queue] |
||
59 | #define WPINGREPLY 33 //[type, fromBot, toBot] |
||
60 | 967 | alevkoy | |
61 | 1843 | bwasserm | int main (void) { |
62 | 1419 | jsexton | |
63 | 1844 | bwasserm | int state, sign, dataLength;
|
64 | 1859 | bwasserm | char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum;
|
65 | 1419 | jsexton | |
66 | 1843 | bwasserm | /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
|
67 | dragonfly_init(ALL_ON); |
||
68 | xbee_init(); |
||
69 | encoders_init(); |
||
70 | lineFollow_init(); |
||
71 | 1844 | bwasserm | rtc_init(SIXTEENTH_SECOND, NULL);
|
72 | 1859 | bwasserm | wl_basic_init_default(); |
73 | wl_set_channel(13);
|
||
74 | |||
75 | 1844 | bwasserm | id = get_robotid(); |
76 | 1843 | bwasserm | sign = 0;
|
77 | |||
78 | 1859 | bwasserm | //Test code
|
79 | state = SROAD; |
||
80 | |||
81 | 1844 | bwasserm | sendBuffer[1] = id;
|
82 | |||
83 | 1843 | bwasserm | while (1) { |
84 | /*DTM Finite State Machine*/
|
||
85 | switch(state){
|
||
86 | case SROAD:/*Following a normal road*/ |
||
87 | 1859 | bwasserm | // sign = lineFollow();
|
88 | 1843 | bwasserm | //other road behaviors
|
89 | //tailgating?
|
||
90 | //read barcode
|
||
91 | 1859 | bwasserm | if((sign & CINTERSECTION) || button1_click()){
|
92 | 1843 | bwasserm | state = SINTERSECTION; |
93 | } |
||
94 | break;
|
||
95 | case SINTERSECTION:/*Entering, and in intersection*/ |
||
96 | //Intersection behaviors
|
||
97 | //queueing
|
||
98 | //no-stop?
|
||
99 | //check wireless
|
||
100 | 1844 | bwasserm | |
101 | 1859 | bwasserm | intersectionNum = 0;
|
102 | |||
103 | 1844 | bwasserm | /*Intersection queue:
|
104 | *Each robot when entering the intersection will check for other robots
|
||
105 | *in the intersection, and insert itself in a queue to go through.
|
||
106 | */
|
||
107 | prevBot = 0;
|
||
108 | nextBot = 0;
|
||
109 | //Sends packet announcing its entry to the intersection
|
||
110 | sendBuffer[0] = WINTERSECTIONENTRY;
|
||
111 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
112 | 1844 | bwasserm | sendBuffer[3] = 0;//From Direction |
113 | 1859 | bwasserm | sendBuffer[4] = 2;//To Direction |
114 | 1844 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
115 | 1859 | bwasserm | orb1_set_color(BLUE); |
116 | 1844 | bwasserm | rtc_reset(); |
117 | while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue |
||
118 | packet = wl_basic_do_default(&dataLength); |
||
119 | 1859 | bwasserm | if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){ |
120 | 1844 | bwasserm | if(packet[3]==id){//Reply for me |
121 | prevBot = packet[1];
|
||
122 | 1859 | bwasserm | orb2_set_color(GREEN); |
123 | 1844 | bwasserm | break;
|
124 | }else{//Someone else got here first, try again |
||
125 | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
||
126 | rtc_reset(); |
||
127 | } |
||
128 | } |
||
129 | } |
||
130 | 1859 | bwasserm | orb1_set_color(PURPLE); |
131 | 1844 | bwasserm | //waits for its turn
|
132 | while(prevBot != 0){ |
||
133 | packet = wl_basic_do_default(&dataLength); |
||
134 | 1859 | bwasserm | if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){ |
135 | 1844 | bwasserm | if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){ |
136 | prevBot = 0;
|
||
137 | 1859 | bwasserm | orb2_set_color(PURPLE); |
138 | 1844 | bwasserm | }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
139 | sendBuffer[0] = WINTERSECTIONREPLY;
|
||
140 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
141 | 1844 | bwasserm | sendBuffer[3] = packet[1]; |
142 | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
||
143 | nextBot = packet[1];
|
||
144 | nextDir = packet[3];
|
||
145 | nextPath = packet[4];
|
||
146 | 1859 | bwasserm | orb2_set_color(BLUE); |
147 | 1844 | bwasserm | } |
148 | } |
||
149 | } |
||
150 | 1859 | bwasserm | orb1_set_color(RED); |
151 | 1844 | bwasserm | //Drives through intersection
|
152 | //Code to choose path through intersection goes in this while loop
|
||
153 | //But here's the code to handle wireless while in the intersection...
|
||
154 | 1859 | bwasserm | while(!button1_click()/*replace with variable to keep track of if in intersection*/){ |
155 | 1844 | bwasserm | packet = wl_basic_do_default(&dataLength); |
156 | 1859 | bwasserm | if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){ |
157 | 1844 | bwasserm | if(packet[0]==WINTERSECTIONEXIT){ |
158 | prevBot = 0;
|
||
159 | 1859 | bwasserm | orb2_set_color(RED); |
160 | 1844 | bwasserm | }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){ |
161 | sendBuffer[0] = WINTERSECTIONREPLY;
|
||
162 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
163 | 1844 | bwasserm | sendBuffer[3] = packet[1]; |
164 | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
||
165 | nextBot = packet[1];
|
||
166 | nextDir = packet[3];
|
||
167 | nextPath = packet[4];
|
||
168 | 1859 | bwasserm | orb2_set_color(YELLOW); |
169 | 1844 | bwasserm | } |
170 | } |
||
171 | if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time |
||
172 | /*if(bots paths won't collide){
|
||
173 | sendBuffer[0] = WINTERSECTIONGO;
|
||
174 | sendBuffer[2] = 0;//Intersection #
|
||
175 | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
||
176 | */
|
||
177 | } |
||
178 | } |
||
179 | //Exits intersection
|
||
180 | sendBuffer[0] = WINTERSECTIONEXIT;
|
||
181 | 1859 | bwasserm | sendBuffer[2] = intersectionNum;//Intersection # |
182 | 1844 | bwasserm | wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
|
183 | 1859 | bwasserm | orb1_set_color(ORANGE); |
184 | 1844 | bwasserm | |
185 | 1843 | bwasserm | if(!(sign & CINTERSECTION)){
|
186 | if(sign & CHIGHWAYROAD){
|
||
187 | state = SHIGHWAY; |
||
188 | }else{
|
||
189 | state = SROAD; |
||
190 | } |
||
191 | } |
||
192 | break;
|
||
193 | case SHIGHWAY:/*On highway*/ |
||
194 | 1859 | bwasserm | // sign = lineFollow();
|
195 | 1843 | bwasserm | //highway behaviors
|
196 | //merging
|
||
197 | //passing
|
||
198 | //read barcode
|
||
199 | break;
|
||
200 | default:
|
||
201 | usb_puts("I got stuck in an unknown state! My state is ");
|
||
202 | usb_puti(state); |
||
203 | } |
||
204 | 967 | alevkoy | } |
205 | |||
206 | 16 | bcoltin | } |