Statistics
| Revision:

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

History | View | Annotate | Download (6.66 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;
12
        static unsigned char *packet;
13

    
14
int main (void) {
15

    
16
        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
17
        dragonfly_init(ALL_ON);
18
        xbee_init();
19
        encoders_init();
20
        lineDrive_init();
21
        rtc_init(SIXTEENTH_SECOND, NULL);        
22
        wl_basic_init_default();
23
        wl_set_channel(13);
24
        
25
        id = get_robotid();
26
        sign = 0;
27
        
28
        //Test code
29
        state = SHIGHWAY;
30

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

    
106
}
107

    
108
void enterIntersectionQueue(){
109
        //Sends packet announcing its entry to the intersection
110
        sendBuffer[0] = WINTERSECTIONENTRY;
111
        sendBuffer[2] = intersectionNum;//Intersection #
112
        sendBuffer[3] = 0;//From Direction
113
        sendBuffer[4] = 2;//To Direction
114
        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
115
        orb1_set_color(BLUE);
116
        stop();
117
        doDrive(0);
118
        rtc_reset();
119
        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
120
                dataLength = 0;
121
                packet = NULL;
122
                packet = wl_basic_do_default(&dataLength);
123
                if(packet && dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
124
                        if(packet[0] == WINTERSECTIONREPLY){
125
                                if(packet[3] == id){//Reply for me
126
                                        prevBot = packet[1];
127
                                        orb2_set_color(GREEN);
128
                                        break;
129
                                }else if(packet[1] != id){//Someone else got here first, try again
130
                                        sendBuffer[0] = WINTERSECTIONENTRY;
131
                                        sendBuffer[2] = intersectionNum;//Intersection #
132
                                        sendBuffer[3] = 0;//From Direction
133
                                        sendBuffer[4] = 2;//To Direction
134
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
135
                                        orb2_set_color(ORANGE);
136
                                        rtc_reset();
137
                                }
138
                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
139
                                sendBuffer[0] = WINTERSECTIONREPLY;
140
                                sendBuffer[2] = intersectionNum;
141
                                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
                                orb2_set_color(BLUE);
147
                                //delay_ms(200);
148
                        }
149
                                delay_ms(0);
150
                }
151
        }
152
}
153

    
154
void waitInIntersectionQueue(){
155
        while(prevBot != 0){
156
                dataLength = 0;
157
                packet = NULL;
158
                packet = wl_basic_do_default(&dataLength);
159
                if(packet && dataLength==PACKET_LENGTH){
160
                        if(packet[2] == intersectionNum){
161
                                if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
162
                                        prevBot = 0;
163
                                        orb2_set_color(PURPLE);
164
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
165
                                        sendBuffer[0] = WINTERSECTIONREPLY;
166
                                        sendBuffer[2] = intersectionNum;
167
                                        sendBuffer[3] = packet[1];
168
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
169
                                        nextBot = packet[1];
170
                                        nextDir = packet[3];
171
                                        nextPath = packet[4];
172
                                        orb2_set_color(BLUE);
173
                                }
174
                        }
175
/*
176
                        if(ISPING(packet)){
177
                                sendBuffer[0] = WPINGREPLY;
178
                                sendBuffer[2] = packet[1];
179
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
180
                                        nextBot = packet[1];
181
                                }
182
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
183
                        }
184
                        if(packet[0]==WPINGREPLY && packet[2]==id){
185
                                prevBot = packet[1];
186
                                pingWaitTime = rtc_get();
187
                        }
188
*/
189
                }
190
                                        
191
                if(prevBot && 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
                }
197
                if(prevBot && pingWaitTime - rtc_get() > 4){
198
                        sendBuffer[0] = WPINGBOT;
199
                        if(pingWaitTime - rtc_get() > 8){
200
                                sendBuffer[0] = WPINGQUEUE;
201
                        }
202
                        sendBuffer[2] = prevBot;
203
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
204
                }
205
/*                                if(prevBot && pingWaitTime - rtc_get() > 12){
206
                        prevBot = 0;
207
                }
208
*/
209
        }
210
}
211

    
212
void driveThroughIntersection(){
213
        //Code to choose path through intersection goes in this while loop
214
        //But here's the code to handle wireless while in the intersection...
215
        start();
216
        turn(DOUBLE, IRIGHT);//Change paramers to variables
217
        while(doDrive(255) != FINISHED){
218
                dataLength = 0;
219
                packet = NULL;
220
                packet = wl_basic_do_default(&dataLength);                
221
                if(dataLength==PACKET_LENGTH){
222
                        if(packet[2]==intersectionNum/*Intersection Num*/){
223
                                if(packet[0]==WINTERSECTIONEXIT){
224
                                        prevBot = 0;
225
                                        orb2_set_color(RED);
226
                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
227
                                        sendBuffer[0] = WINTERSECTIONREPLY;
228
                                        sendBuffer[2] = intersectionNum;//Intersection #
229
                                        sendBuffer[3] = packet[1];
230
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
231
                                        nextBot = packet[1];
232
                                        nextDir = packet[3];
233
                                        nextPath = packet[4];
234
                                        orb2_set_color(YELLOW);
235
                                }
236
                        }
237
                        if(ISPING(packet)){
238
                                sendBuffer[0] = WPINGREPLY;
239
                                sendBuffer[2] = packet[1];
240
                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
241
                                        nextBot = packet[1];
242
                                }
243
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
244
                        }
245
                }
246
                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
247
                        /*if(bots paths won't collide){
248
                                sendBuffer[0] = WINTERSECTIONGO;
249
                                sendBuffer[2] = 0;//Intersection #
250
                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
251
                        */
252
                }
253
        }
254
}