Statistics
| Revision:

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

History | View | Annotate | Download (6.47 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
int main (void) {
11

    
12
        int state, sign, dataLength, pingWaitTime;
13
        char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, nextDir, nextPath, intersectionNum;
14
        unsigned char *packet;
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
        while (1) {
34
                /*DTM Finite State Machine*/
35
                switch(state){
36
                case SROAD:/*Following a normal road*/
37
//                        sign = lineFollow();
38
                        //other road behaviors
39
                                //tailgating?
40
                        //read barcode
41
//                        doDrive(255);
42
                        if((sign & CINTERSECTION) || button1_click()){
43
                                state = SINTERSECTION;
44
                        }
45
                        break;
46
                case SINTERSECTION:/*Entering, and in intersection*/
47
                        //Intersection behaviors
48
                                //queueing
49
                                //no-stop?
50
                        //check wireless
51
                        
52
                        intersectionNum = 0;
53

    
54
                        /*Intersection queue:
55
                         *Each robot when entering the intersection will check for other robots
56
                         *in the intersection, and insert itself in a queue to go through.
57
                         */
58
                        prevBot = 0;
59
                        nextBot = 0;
60
                        //Sends packet announcing its entry to the intersection
61
                        sendBuffer[0] = WINTERSECTIONENTRY;
62
                        sendBuffer[2] = intersectionNum;//Intersection #
63
                        sendBuffer[3] = 0;//From Direction
64
                        sendBuffer[4] = 2;//To Direction
65
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
66
                        orb1_set_color(BLUE);
67
//                        stop();
68
                        rtc_reset();
69
                        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
70
                                dataLength = 0;
71
                                packet = wl_basic_do_default(&dataLength);
72
                                if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum){
73
                                        if(packet[0] == WINTERSECTIONREPLY){
74
                                                if(packet[3] == id){//Reply for me
75
                                                        prevBot = packet[1];
76
                                                        orb2_set_color(GREEN);
77
                                                        break;
78
                                                }else if(packet[1] != id){//Someone else got here first, try again
79
                                                        sendBuffer[0] = WINTERSECTIONENTRY;
80
                                                        sendBuffer[2] = intersectionNum;//Intersection #
81
                                                        sendBuffer[3] = 0;//From Direction
82
                                                        sendBuffer[4] = 2;//To Direction
83
                                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
84
                                                        orb2_set_color(ORANGE);
85
                                                        rtc_reset();
86
                                                }
87
                                        }else /*if(packet[0]==WINTERSECTIONENTRY && nextBot==0 && prevBot!=packet[1]){
88
                                                sendBuffer[0] = WINTERSECTIONREPLY;
89
                                                sendBuffer[2] = intersectionNum;
90
                                                sendBuffer[3] = packet[1];
91
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
92
                                                nextBot = packet[1];
93
                                                nextDir = packet[3];
94
                                                nextPath = packet[4];
95
                                                orb2_set_color(BLUE);
96
                                                delay_ms(200);
97
                                        }*/
98
                                                delay_ms(0);
99
                                }
100
                        }
101
                        orb1_set_color(PURPLE);
102
                        //waits for its turn
103
                        while(prevBot != 0){
104
                                dataLength = 0;
105
                                packet = wl_basic_do_default(&dataLength);
106
                                if(dataLength==PACKET_LENGTH){
107
                                        if(packet[2] == intersectionNum){
108
                                                if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
109
                                                        prevBot = 0;
110
                                                        orb2_set_color(PURPLE);
111
                                                }/*else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
112
                                                        sendBuffer[0] = WINTERSECTIONREPLY;
113
                                                        sendBuffer[2] = intersectionNum;
114
                                                        sendBuffer[3] = packet[1];
115
                                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
116
                                                        nextBot = packet[1];
117
                                                        nextDir = packet[3];
118
                                                        nextPath = packet[4];
119
                                                        orb2_set_color(BLUE);
120
                                                }*/
121
                                        }
122
/*                                        if(ISPING(packet)){
123
                                                sendBuffer[0] = WPINGREPLY;
124
                                                sendBuffer[2] = packet[1];
125
                                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
126
                                                        nextBot = packet[1];
127
                                                }
128
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
129
                                        }
130
                                        if(packet[0]==WPINGREPLY && packet[2]==id){
131
                                                prevBot = packet[1];
132
                                                pingWaitTime = rtc_get();
133
                                        }
134
*/                                }
135
                                                        
136
                                if(prevBot && rtc_get() << 12 == 0){
137
                                        sendBuffer[0] = WPINGBOT;
138
                                        sendBuffer[2] = prevBot;
139
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
140
                                        pingWaitTime = rtc_get();
141
                                }
142
                                if(prevBot && pingWaitTime - rtc_get() > 4){
143
                                        sendBuffer[0] = WPINGBOT;
144
                                        if(pingWaitTime - rtc_get() > 8){
145
                                                sendBuffer[0] = WPINGQUEUE;
146
                                        }
147
                                        sendBuffer[2] = prevBot;
148
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
149
                                }
150
/*                                if(prevBot && pingWaitTime - rtc_get() > 12){
151
                                        prevBot = 0;
152
                                }
153
*/                        }
154
                        orb1_set_color(RED);
155
                        //Drives through intersection
156
                        //Code to choose path through intersection goes in this while loop
157
                        //But here's the code to handle wireless while in the intersection...
158
//                        start();
159
//                        turn(DOUBLE, ILEFT);//Change paramers to variables
160
                        while(!button1_click()/*replace with variable to keep track of if in intersection*/){
161
//                                doDrive(255);
162
                                packet = wl_basic_do_default(&dataLength);                
163
                                if(dataLength==PACKET_LENGTH){
164
                                        if(packet[2]==intersectionNum/*Intersection Num*/){
165
                                                if(packet[0]==WINTERSECTIONEXIT){
166
                                                        prevBot = 0;
167
                                                        orb2_set_color(RED);
168
                                                }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
169
                                                        sendBuffer[0] = WINTERSECTIONREPLY;
170
                                                        sendBuffer[2] = intersectionNum;//Intersection #
171
                                                        sendBuffer[3] = packet[1];
172
                                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
173
                                                        nextBot = packet[1];
174
                                                        nextDir = packet[3];
175
                                                        nextPath = packet[4];
176
                                                        orb2_set_color(YELLOW);
177
                                                }
178
                                        }
179
                                        if(ISPING(packet)){
180
                                                sendBuffer[0] = WPINGREPLY;
181
                                                sendBuffer[2] = packet[1];
182
                                                if(packet[0]==WPINGQUEUE && packet[3]==nextBot){
183
                                                        nextBot = packet[1];
184
                                                }
185
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
186
                                        }
187
                                }
188
                                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
189
                                        /*if(bots paths won't collide){
190
                                                sendBuffer[0] = WINTERSECTIONGO;
191
                                                sendBuffer[2] = 0;//Intersection #
192
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
193
                                        */
194
                                }
195
                        }
196
                        //Exits intersection
197
                        sendBuffer[0] = WINTERSECTIONEXIT;
198
                        sendBuffer[2] = intersectionNum;//Intersection #
199
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
200
                        orb1_set_color(ORANGE);
201
                        
202
                        if(!(sign & CINTERSECTION)){
203
                                if(sign & CHIGHWAYROAD){
204
                                        state = SHIGHWAY;
205
                                }else{
206
                                        state = SROAD;
207
                                }
208
                        }
209
                        break;
210
                case SHIGHWAY:/*On highway*/
211
                        driveHighway();
212
                        break;
213
                default:
214
                        usb_puts("I got stuck in an unknown state! My state is ");
215
                        usb_puti(state);
216
                }
217
        }
218

    
219
}