Statistics
| Revision:

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

History | View | Annotate | Download (7.54 KB)

1
/*
2
 * main.c for Traffic Navigation
3
 * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG
4
 *
5
 * Author: Colony Project, CMU Robotics Club
6
 */
7

    
8
#include "traffic_navigation.h"
9

    
10
>>>>>>> .r1869
11
int main (void) {
12

    
13
        void enterIntersectionQueue(int sign);
14
        void waitInIntersectionQueue();
15
        void driveThroughIntersection(int sign);
16
        
17
int main (void) {
18
        
19
        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
20
        dragonfly_init(ALL_ON);
21
        xbee_init();
22
        encoders_init();
23
        lineDrive_init();
24
        rtc_init(SIXTEENTH_SECOND, NULL);        
25
        wl_basic_init_default();
26
        wl_set_channel(13);
27
        initializeData();
28
        
29
        id = get_robotid();
30
        sign = 0;
31
        
32
        //Test code
33
        state = SROAD;
34

    
35
        sendBuffer[1] = id;
36

    
37
        state = SHIGHWAY;
38

    
39
        int startLeft = 0, startRight = 0;
40

    
41

    
42
        while (1) {
43
                /*DTM Finite State Machine*/
44
                switch(state){
45
                case SROAD:/*Following a normal road*/
46
<<<<<<< .mine
47
                        sign = lineFollow(200);
48
=======
49
//                        sign = lineFollow();
50
>>>>>>> .r1869
51
                        //other road behaviors
52
                                //tailgating?
53
                        //read barcode
54
                        sign = doDrive(200);
55
                        if(button1_click()){
56
                                state = SINTERSECTION;
57
                        }
58
                        break;
59
                case SINTERSECTION:/*Entering, and in intersection*/
60
                        intersectionNum = 0;
61
                        /*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
                        
68
                        sign = 0; //Test code until barcodes integrated
69
                        
70
                        enterIntersectionQueue(sign);
71
                        
72
                        orb1_set_color(PURPLE);
73
                        //waits for its turn
74
                        
75
                        waitInIntersectionQueue();
76
                        
77
                        orb1_set_color(RED);
78
                        //Drives through intersection
79
                        driveThroughIntersection(sign);
80
                        
81
                        //Exits intersection
82
                        sendBuffer[0] = WINTERSECTIONEXIT;
83
                        sendBuffer[2] = intersectionNum;//Intersection #
84
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
85
                        orb1_set_color(ORANGE);
86
                        while(1){
87
                                stop();
88
                                sign = doDrive(200);
89
                                if(button1_click()){
90
                                        start();
91
                                        state = SHIGHWAY;
92
                                        break;
93
                                }
94
                                if(button2_click()){
95
                                        start();
96
                                        state = SROAD;
97
                                        break;
98
                                }
99
                        }
100
                        break;
101
                case SHIGHWAY:/*On highway*/
102
                        orb1_set_color(CYAN);
103
                        while(!button1_click()){
104
                                highwayFSM();
105
                        }
106
                        state = SINTERSECTION;
107
                        break;
108
                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
                default:
143
                        usb_puts("I got stuck in an unknown state! My state is ");
144
                        usb_puti(state);
145
                }
146
        }
147

    
148
}
149

    
150
void enterIntersectionQueue(int sign){
151
                
152
        //Choose turn direction
153
        intersectionNum = getIntersectType(sign);
154
        turnDir = validateTurn(sign, getTurnType(4));
155
        //Sends packet announcing its entry to the intersection
156
        sendBuffer[0] = WINTERSECTIONENTRY;
157
        sendBuffer[2] = intersectionNum;//Intersection #
158
        sendBuffer[3] = getIntersectPos(sign);
159
        sendBuffer[4] = turnDir;
160
        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
                                        sendBuffer[2] = intersectionNum;
178
                                        sendBuffer[3] = getIntersectPos(sign);
179
                                        sendBuffer[4] = turnDir;
180
                                        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
void driveThroughIntersection(int sign){
259
        //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
        turn(getIntersectType(0), turnDir);
263
        while(doDrive(200) != FINISHED){
264
                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
}