Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / main.c @ 1949

History | View | Annotate | Download (7.54 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 1869 bwasserm
 * Author: Colony Project, CMU Robotics Club
6
 */
7 1843 bwasserm
8 1872 josephle
#include "traffic_navigation.h"
9 16 bcoltin
10 1949 dgurjar
>>>>>>> .r1869
11
int main (void) {
12 1888 bwasserm
13 1896 bwasserm
        void enterIntersectionQueue(int sign);
14
        void waitInIntersectionQueue();
15
        void driveThroughIntersection(int sign);
16
17 1843 bwasserm
int main (void) {
18 1896 bwasserm
19 1843 bwasserm
        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
20
        dragonfly_init(ALL_ON);
21
        xbee_init();
22
        encoders_init();
23 1876 markwill
        lineDrive_init();
24 1844 bwasserm
        rtc_init(SIXTEENTH_SECOND, NULL);
25 1859 bwasserm
        wl_basic_init_default();
26
        wl_set_channel(13);
27 1896 bwasserm
        initializeData();
28 1859 bwasserm
29 1844 bwasserm
        id = get_robotid();
30 1843 bwasserm
        sign = 0;
31
32 1859 bwasserm
        //Test code
33 1901 bwasserm
        state = SROAD;
34 1859 bwasserm
35 1844 bwasserm
        sendBuffer[1] = id;
36 1901 bwasserm
37 1949 dgurjar
        state = SHIGHWAY;
38
39
        int startLeft = 0, startRight = 0;
40
41
42 1843 bwasserm
        while (1) {
43
                /*DTM Finite State Machine*/
44
                switch(state){
45
                case SROAD:/*Following a normal road*/
46 1949 dgurjar
<<<<<<< .mine
47
                        sign = lineFollow(200);
48
=======
49 1859 bwasserm
//                        sign = lineFollow();
50 1949 dgurjar
>>>>>>> .r1869
51 1843 bwasserm
                        //other road behaviors
52
                                //tailgating?
53
                        //read barcode
54 1901 bwasserm
                        sign = doDrive(200);
55
                        if(button1_click()){
56 1843 bwasserm
                                state = SINTERSECTION;
57
                        }
58
                        break;
59
                case SINTERSECTION:/*Entering, and in intersection*/
60 1859 bwasserm
                        intersectionNum = 0;
61 1844 bwasserm
                        /*Intersection queue:
62
                         *Each robot when entering the intersection will check for other robots
63
                         *in the intersection, and insert itself in a queue to go through.
64
                         */
65
                        prevBot = 0;
66
                        nextBot = 0;
67 1888 bwasserm
68 1913 bwasserm
                        sign = 0; //Test code until barcodes integrated
69 1896 bwasserm
70
                        enterIntersectionQueue(sign);
71
72 1859 bwasserm
                        orb1_set_color(PURPLE);
73 1844 bwasserm
                        //waits for its turn
74 1888 bwasserm
75
                        waitInIntersectionQueue();
76
77 1859 bwasserm
                        orb1_set_color(RED);
78 1844 bwasserm
                        //Drives through intersection
79 1896 bwasserm
                        driveThroughIntersection(sign);
80 1888 bwasserm
81 1844 bwasserm
                        //Exits intersection
82
                        sendBuffer[0] = WINTERSECTIONEXIT;
83 1859 bwasserm
                        sendBuffer[2] = intersectionNum;//Intersection #
84 1844 bwasserm
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
85 1859 bwasserm
                        orb1_set_color(ORANGE);
86 1888 bwasserm
                        while(1){
87 1891 bwasserm
                                stop();
88 1901 bwasserm
                                sign = doDrive(200);
89 1913 bwasserm
                                if(button1_click()){
90 1891 bwasserm
                                        start();
91 1913 bwasserm
                                        state = SHIGHWAY;
92
                                        break;
93 1843 bwasserm
                                }
94 1913 bwasserm
                                if(button2_click()){
95
                                        start();
96
                                        state = SROAD;
97
                                        break;
98
                                }
99 1843 bwasserm
                        }
100
                        break;
101
                case SHIGHWAY:/*On highway*/
102 1901 bwasserm
                        orb1_set_color(CYAN);
103
                        while(!button1_click()){
104 1891 bwasserm
                                highwayFSM();
105 1901 bwasserm
                        }
106
                        state = SINTERSECTION;
107 1888 bwasserm
                        break;
108 1949 dgurjar
                case PASSLEFT:/*Passing left*/
109
110
                        motor_l_set(FORWARD, 180);
111
                        motor_r_set(FORWARD, 250);
112
113
                        orb_set_color(YELLOW);
114
115
                        usb_puti(startRight);
116
                        usb_puts("\n");
117
                        usb_puti(startLeft);
118
                        usb_puts("\n");
119
                        usb_puts("\n");
120
                        usb_puti((encoder_get_x(RIGHT) - startRight) - (encoder_get_x(LEFT) - startLeft));
121
                        usb_puts("\n\n");
122
123
                        //if((encoder_get_x(RIGHT) - startRight) - (encoder_get_x(LEFT) - startLeft) > 200)
124
                        if(!onLine())
125
                        {
126
                                state = FINDLINE;
127
                                delay_ms(HALF_SECOND);
128
                        }
129
                        break;
130
                case PASSRIGHT:/*Passing right*/
131
                        break;
132
                case FINDLINE:/*Drive straight until a line is falled*/
133
                        motor_l_set(FORWARD, 240);
134
                        motor_r_set(FORWARD, 240);
135
136
                        orb_set_color(RED);
137
138
                        if(onLine())
139
                                orb_set_color(ORB_OFF);
140
                                state = SHIGHWAY;
141
                        break;
142 1843 bwasserm
                default:
143
                        usb_puts("I got stuck in an unknown state! My state is ");
144
                        usb_puti(state);
145
                }
146 967 alevkoy
        }
147
148 16 bcoltin
}
149 1888 bwasserm
150 1896 bwasserm
void enterIntersectionQueue(int sign){
151
152
        //Choose turn direction
153
        intersectionNum = getIntersectType(sign);
154 1913 bwasserm
        turnDir = validateTurn(sign, getTurnType(4));
155 1888 bwasserm
        //Sends packet announcing its entry to the intersection
156
        sendBuffer[0] = WINTERSECTIONENTRY;
157
        sendBuffer[2] = intersectionNum;//Intersection #
158 1896 bwasserm
        sendBuffer[3] = getIntersectPos(sign);
159
        sendBuffer[4] = turnDir;
160 1888 bwasserm
        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
161
        orb1_set_color(BLUE);
162
        stop();
163
        doDrive(0);
164
        rtc_reset();
165
        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
166
                dataLength = 0;
167
                packet = NULL;
168
                packet = wl_basic_do_default(&dataLength);
169
                if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
170
                        if(packet[0] == WINTERSECTIONREPLY){
171
                                if(packet[3] == id){//Reply for me
172
                                        prevBot = packet[1];
173
                                        orb2_set_color(GREEN);
174
                                        break;
175
                                }else if(packet[1] != id){//Someone else got here first, try again
176
                                        sendBuffer[0] = WINTERSECTIONENTRY;
177 1896 bwasserm
                                        sendBuffer[2] = intersectionNum;
178
                                        sendBuffer[3] = getIntersectPos(sign);
179
                                        sendBuffer[4] = turnDir;
180 1888 bwasserm
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
181
                                        orb2_set_color(ORANGE);
182
                                        rtc_reset();
183
                                }
184
                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
185
                                sendBuffer[0] = WINTERSECTIONREPLY;
186
                                sendBuffer[2] = intersectionNum;
187
                                sendBuffer[3] = packet[1];
188
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
189
                                nextBot = packet[1];
190
                                nextDir = packet[3];
191
                                nextPath = packet[4];
192
                                orb2_set_color(BLUE);
193
                                //delay_ms(200);
194
                        }
195
                                delay_ms(0);
196
                }
197
        }
198
}
199
200
void waitInIntersectionQueue(){
201
        while(prevBot != 0){
202
                dataLength = 0;
203
                packet = NULL;
204
                packet = wl_basic_do_default(&dataLength);
205
                if(packet && dataLength==PACKET_LENGTH){
206
                        if(packet[2] == intersectionNum){
207
                                if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
208
                                        prevBot = 0;
209
                                        orb2_set_color(PURPLE);
210
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
211
                                        sendBuffer[0] = WINTERSECTIONREPLY;
212
                                        sendBuffer[2] = intersectionNum;
213
                                        sendBuffer[3] = packet[1];
214
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
215
                                        nextBot = packet[1];
216
                                        nextDir = packet[3];
217
                                        nextPath = packet[4];
218
                                        orb2_set_color(BLUE);
219
                                }
220
                        }
221
/*
222
                        if(ISPING(packet)){
223
                                sendBuffer[0] = WPINGREPLY;
224
                                sendBuffer[2] = packet[1];
225
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
226
                                        nextBot = packet[1];
227
                                }
228
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
229
                        }
230
                        if(packet[0]==WPINGREPLY && packet[2]==id){
231
                                prevBot = packet[1];
232
                                pingWaitTime = rtc_get();
233
                        }
234
*/
235
                }
236
237
                if(prevBot && rtc_get() << 12 == 0){
238
                        sendBuffer[0] = WPINGBOT;
239
                        sendBuffer[2] = prevBot;
240
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
241
                        pingWaitTime = rtc_get();
242
                }
243
                if(prevBot && pingWaitTime - rtc_get() > 4){
244
                        sendBuffer[0] = WPINGBOT;
245
                        if(pingWaitTime - rtc_get() > 8){
246
                                sendBuffer[0] = WPINGQUEUE;
247
                        }
248
                        sendBuffer[2] = prevBot;
249
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
250
                }
251
/*                                if(prevBot && pingWaitTime - rtc_get() > 12){
252
                        prevBot = 0;
253
                }
254
*/
255
        }
256
}
257
258 1896 bwasserm
void driveThroughIntersection(int sign){
259 1888 bwasserm
        //Code to choose path through intersection goes in this while loop
260
        //But here's the code to handle wireless while in the intersection...
261
        start();
262 1912 pdeo
        turn(getIntersectType(0), turnDir);
263 1901 bwasserm
        while(doDrive(200) != FINISHED){
264 1888 bwasserm
                dataLength = 0;
265
                packet = NULL;
266
                packet = wl_basic_do_default(&dataLength);
267
                if(dataLength==PACKET_LENGTH){
268
                        if(packet[2]==intersectionNum/*Intersection Num*/){
269
                                if(packet[0]==WINTERSECTIONEXIT){
270
                                        prevBot = 0;
271
                                        orb2_set_color(RED);
272
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
273
                                        sendBuffer[0] = WINTERSECTIONREPLY;
274
                                        sendBuffer[2] = intersectionNum;//Intersection #
275
                                        sendBuffer[3] = packet[1];
276
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
277
                                        nextBot = packet[1];
278
                                        nextDir = packet[3];
279
                                        nextPath = packet[4];
280
                                        orb2_set_color(YELLOW);
281
                                }
282
                        }
283
                        if(ISPING(packet)){
284
                                sendBuffer[0] = WPINGREPLY;
285
                                sendBuffer[2] = packet[1];
286
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
287
                                        nextBot = packet[1];
288
                                }
289
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
290
                        }
291
                }
292
                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
293
                        /*if(bots paths won't collide){
294
                                sendBuffer[0] = WINTERSECTIONGO;
295
                                sendBuffer[2] = 0;//Intersection #
296
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
297
                        */
298
                }
299
        }
300 1905 pdeo
}