Statistics
| Revision:

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

History | View | Annotate | Download (6.58 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 = SROAD;
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
//                        sign = lineFollow();
92
                        //highway behaviors
93
                                //merging
94
                                //passing
95
                        //read barcode
96
                        break;
97
                        break;
98
                default:
99
                        usb_puts("I got stuck in an unknown state! My state is ");
100
                        usb_puti(state);
101
                }
102
        }
103

    
104
}
105

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

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

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