Statistics
| Revision:

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

History | View | Annotate | Download (6.9 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
        static int state, sign, dataLength, pingWaitTime;
11
        static char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum, turnDir;
12
        static unsigned char *packet;
13

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

    
36
        sendBuffer[1] = id;
37
/*
38
        doDrive(255);
39
        turn(DOUBLE, ILEFT);
40
*/        
41
/*        while(1)
42
                doDrive(255);
43
        */
44
        
45
        while (1) {
46
                /*DTM Finite State Machine*/
47
                switch(state){
48
                case SROAD:/*Following a normal road*/
49
//                        sign = lineFollow();
50
                        //other road behaviors
51
                                //tailgating?
52
                        //read barcode
53
                        sign = doDrive(255);
54
                        if((sign >= 0) || button1_click()){
55
                                state = SINTERSECTION;
56
                        }
57
                        break;
58
                case SINTERSECTION:/*Entering, and in intersection*/
59
                        intersectionNum = 0;
60
                        /*Intersection queue:
61
                         *Each robot when entering the intersection will check for other robots
62
                         *in the intersection, and insert itself in a queue to go through.
63
                         */
64
                        prevBot = 0;
65
                        nextBot = 0;
66
                        
67
                        sign = 14; //Test code until barcodes integrated
68
                        
69
                        enterIntersectionQueue(sign);
70
                        
71
                        orb1_set_color(PURPLE);
72
                        //waits for its turn
73
                        
74
                        waitInIntersectionQueue();
75
                        
76
                        orb1_set_color(RED);
77
                        //Drives through intersection
78
                        driveThroughIntersection(sign);
79
                        
80
                        //Exits intersection
81
                        sendBuffer[0] = WINTERSECTIONEXIT;
82
                        sendBuffer[2] = intersectionNum;//Intersection #
83
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
84
                        orb1_set_color(ORANGE);
85
                        while(1){
86
                                stop();
87
                                sign = doDrive(255);
88
                                if((sign >= 0) || button1_click()){
89
                                        start();
90
                                        if(0){
91
                                                state = SHIGHWAY;
92
                                                break;
93
                                        }else{
94
                                                state = SROAD;
95
                                                break;
96
                                        }
97
                                }
98
                        }
99
                        break;
100
                case SHIGHWAY:/*On highway*/
101
                //        while(!button1_click()){
102
                                highwayFSM();
103
                //        }
104
                        //state = SINTERSECTION;
105
                        break;
106
                default:
107
                        usb_puts("I got stuck in an unknown state! My state is ");
108
                        usb_puti(state);
109
                }
110
        }
111

    
112
}
113

    
114
void enterIntersectionQueue(int sign){
115
                
116
        //Choose turn direction
117
        intersectionNum = getIntersectType(sign);
118
        turnDir = validateTurn(sign, getTurnType(14));
119
        
120
        //Sends packet announcing its entry to the intersection
121
        sendBuffer[0] = WINTERSECTIONENTRY;
122
        sendBuffer[2] = intersectionNum;//Intersection #
123
        sendBuffer[3] = getIntersectPos(sign);
124
        sendBuffer[4] = turnDir;
125
        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
126
        orb1_set_color(BLUE);
127
        stop();
128
        doDrive(0);
129
        rtc_reset();
130
        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
131
                dataLength = 0;
132
                packet = NULL;
133
                packet = wl_basic_do_default(&dataLength);
134
                if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
135
                        if(packet[0] == WINTERSECTIONREPLY){
136
                                if(packet[3] == id){//Reply for me
137
                                        prevBot = packet[1];
138
                                        orb2_set_color(GREEN);
139
                                        break;
140
                                }else if(packet[1] != id){//Someone else got here first, try again
141
                                        sendBuffer[0] = WINTERSECTIONENTRY;
142
                                        sendBuffer[2] = intersectionNum;
143
                                        sendBuffer[3] = getIntersectPos(sign);
144
                                        sendBuffer[4] = turnDir;
145
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
146
                                        orb2_set_color(ORANGE);
147
                                        rtc_reset();
148
                                }
149
                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
150
                                sendBuffer[0] = WINTERSECTIONREPLY;
151
                                sendBuffer[2] = intersectionNum;
152
                                sendBuffer[3] = packet[1];
153
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
154
                                nextBot = packet[1];
155
                                nextDir = packet[3];
156
                                nextPath = packet[4];
157
                                orb2_set_color(BLUE);
158
                                //delay_ms(200);
159
                        }
160
                                delay_ms(0);
161
                }
162
        }
163
}
164

    
165
void waitInIntersectionQueue(){
166
        while(prevBot != 0){
167
                dataLength = 0;
168
                packet = NULL;
169
                packet = wl_basic_do_default(&dataLength);
170
                if(packet && dataLength==PACKET_LENGTH){
171
                        if(packet[2] == intersectionNum){
172
                                if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
173
                                        prevBot = 0;
174
                                        orb2_set_color(PURPLE);
175
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
176
                                        sendBuffer[0] = WINTERSECTIONREPLY;
177
                                        sendBuffer[2] = intersectionNum;
178
                                        sendBuffer[3] = packet[1];
179
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
180
                                        nextBot = packet[1];
181
                                        nextDir = packet[3];
182
                                        nextPath = packet[4];
183
                                        orb2_set_color(BLUE);
184
                                }
185
                        }
186
/*
187
                        if(ISPING(packet)){
188
                                sendBuffer[0] = WPINGREPLY;
189
                                sendBuffer[2] = packet[1];
190
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
191
                                        nextBot = packet[1];
192
                                }
193
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
194
                        }
195
                        if(packet[0]==WPINGREPLY && packet[2]==id){
196
                                prevBot = packet[1];
197
                                pingWaitTime = rtc_get();
198
                        }
199
*/
200
                }
201
                                        
202
                if(prevBot && rtc_get() << 12 == 0){
203
                        sendBuffer[0] = WPINGBOT;
204
                        sendBuffer[2] = prevBot;
205
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
206
                        pingWaitTime = rtc_get();
207
                }
208
                if(prevBot && pingWaitTime - rtc_get() > 4){
209
                        sendBuffer[0] = WPINGBOT;
210
                        if(pingWaitTime - rtc_get() > 8){
211
                                sendBuffer[0] = WPINGQUEUE;
212
                        }
213
                        sendBuffer[2] = prevBot;
214
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
215
                }
216
/*                                if(prevBot && pingWaitTime - rtc_get() > 12){
217
                        prevBot = 0;
218
                }
219
*/
220
        }
221
}
222

    
223
void driveThroughIntersection(int sign){
224
        //Code to choose path through intersection goes in this while loop
225
        //But here's the code to handle wireless while in the intersection...
226
        start();
227
        turn(getIntersectType(sign), turnDir);
228
        while(doDrive(255) != FINISHED){
229
                dataLength = 0;
230
                packet = NULL;
231
                packet = wl_basic_do_default(&dataLength);                
232
                if(dataLength==PACKET_LENGTH){
233
                        if(packet[2]==intersectionNum/*Intersection Num*/){
234
                                if(packet[0]==WINTERSECTIONEXIT){
235
                                        prevBot = 0;
236
                                        orb2_set_color(RED);
237
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
238
                                        sendBuffer[0] = WINTERSECTIONREPLY;
239
                                        sendBuffer[2] = intersectionNum;//Intersection #
240
                                        sendBuffer[3] = packet[1];
241
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
242
                                        nextBot = packet[1];
243
                                        nextDir = packet[3];
244
                                        nextPath = packet[4];
245
                                        orb2_set_color(YELLOW);
246
                                }
247
                        }
248
                        if(ISPING(packet)){
249
                                sendBuffer[0] = WPINGREPLY;
250
                                sendBuffer[2] = packet[1];
251
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
252
                                        nextBot = packet[1];
253
                                }
254
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
255
                        }
256
                }
257
                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
258
                        /*if(bots paths won't collide){
259
                                sendBuffer[0] = WINTERSECTIONGO;
260
                                sendBuffer[2] = 0;//Intersection #
261
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
262
                        */
263
                }
264
        }
265
}