Statistics
| Revision:

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

History | View | Annotate | Download (6.33 KB)

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

    
8
#include <dragonfly_lib.h>
9
#include <wl_basic.h>
10
#include "../linefollowing/lineFollow.h"
11

    
12

    
13
/*States*/
14
#define SROAD 0
15
#define SINTERSECTION 10
16
#define SHIGHWAY 20
17

    
18
/*Sign Codes
19
 * bitwise OR labels to create a barcode or read one
20
 * There should be macros to extract these probably
21
 * The bits will be stored in some variable (char or short)
22
 * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD
23
 * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION
24
 */
25
#define CROAD 0x0 //0b
26
#define CINTERSECTION 0x1 //1b
27
#define CNORMALROAD 0x0 //00b
28
#define CHIGHWAYROAD 0x2 //10b
29
#define C4WAY 0x0 //00b
30
#define C3WAY 0x2 //10b
31
#define CNORTH 0x0 //0000b
32
#define CEAST 0x4 //0100b
33
#define CSOUTH 0x8 //1000b
34
#define CWEST 0x12 //1100b
35

    
36
/*Wireless Packet Types
37
 * The first byte of any wireless packet should be one of these types.
38
 * Each type will have its own structure
39
 * The second byte should be the id of the bot sending the packet
40
 * The third byte should be the number of the intersection or road that
41
 *   the packet pertains to
42
 */
43
#define PACKET_LENGTH 5
44
#define WROADENTRY 0 //[type, bot, road]
45
#define WROADREPLY 1 //[type, fromBot, road, toBot]
46
#define WROADEXIT 2 //[type, bot, road]
47
#define WROADSTOP 3 //[type, bot, road]
48
#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir]
49
#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]
50
#define WINTERSECTIONEXIT 12 //[type, bot, intersection]
51
#define WINTERSECTIONGO 13 //[type, bot, intersection]
52
#define WINTERSECTIONPOLICEENTRY 14
53
#define WHIGHWAYENTRY 20 //[type, bot, highway]
54
#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]
55
#define WHIGHWAYEXIT 22 //[type, bot, highway]
56
#define WPINGGLOBAL 30 //[type, bot]
57
#define WPINGBOT 31 //[type, fromBot, toBot]
58
#define WPINGQUEUE 32 //[type, bot, queue]
59
#define WPINGREPLY 33 //[type, fromBot, toBot]
60

    
61
int main (void) {
62

    
63
        int state, sign, dataLength;
64
        char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum;
65

    
66
        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
67
        dragonfly_init(ALL_ON);
68
        xbee_init();
69
        encoders_init();
70
        lineFollow_init();
71
        rtc_init(SIXTEENTH_SECOND, NULL);        
72
        wl_basic_init_default();
73
        wl_set_channel(13);
74
        
75
        id = get_robotid();
76
        sign = 0;
77
        
78
        //Test code
79
        state = SROAD;
80

    
81
        sendBuffer[1] = id;
82

    
83
        while (1) {
84
                /*DTM Finite State Machine*/
85
                switch(state){
86
                case SROAD:/*Following a normal road*/
87
//                        sign = lineFollow();
88
                        //other road behaviors
89
                                //tailgating?
90
                        //read barcode
91
                        if((sign & CINTERSECTION) || button1_click()){
92
                                state = SINTERSECTION;
93
                        }
94
                        break;
95
                case SINTERSECTION:/*Entering, and in intersection*/
96
                        //Intersection behaviors
97
                                //queueing
98
                                //no-stop?
99
                        //check wireless
100
                        
101
                        intersectionNum = 0;
102

    
103
                        /*Intersection queue:
104
                         *Each robot when entering the intersection will check for other robots
105
                         *in the intersection, and insert itself in a queue to go through.
106
                         */
107
                        prevBot = 0;
108
                        nextBot = 0;
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
                        rtc_reset();
117
                        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue
118
                                packet = wl_basic_do_default(&dataLength);
119
                                if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){
120
                                        if(packet[3]==id){//Reply for me
121
                                                prevBot = packet[1];
122
                                                orb2_set_color(GREEN);
123
                                                break;
124
                                        }else{//Someone else got here first, try again
125
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
126
                                                rtc_reset();
127
                                        }
128
                                }
129
                        }
130
                        orb1_set_color(PURPLE);
131
                        //waits for its turn
132
                        while(prevBot != 0){
133
                                packet = wl_basic_do_default(&dataLength);                
134
                                if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){
135
                                        if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){
136
                                                prevBot = 0;
137
                                                orb2_set_color(PURPLE);
138
                                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
139
                                                sendBuffer[0] = WINTERSECTIONREPLY;
140
                                                sendBuffer[2] = intersectionNum;//Intersection #
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
                                        }
148
                                }
149
                        }
150
                        orb1_set_color(RED);
151
                        //Drives through intersection
152
                        //Code to choose path through intersection goes in this while loop
153
                        //But here's the code to handle wireless while in the intersection...
154
                        while(!button1_click()/*replace with variable to keep track of if in intersection*/){
155
                                packet = wl_basic_do_default(&dataLength);                
156
                                if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){
157
                                        if(packet[0]==WINTERSECTIONEXIT){
158
                                                prevBot = 0;
159
                                                orb2_set_color(RED);
160
                                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){
161
                                                sendBuffer[0] = WINTERSECTIONREPLY;
162
                                                sendBuffer[2] = intersectionNum;//Intersection #
163
                                                sendBuffer[3] = packet[1];
164
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
165
                                                nextBot = packet[1];
166
                                                nextDir = packet[3];
167
                                                nextPath = packet[4];
168
                                                orb2_set_color(YELLOW);
169
                                        }
170
                                }
171
                                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time
172
                                        /*if(bots paths won't collide){
173
                                                sendBuffer[0] = WINTERSECTIONGO;
174
                                                sendBuffer[2] = 0;//Intersection #
175
                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
176
                                        */
177
                                }
178
                        }
179
                        //Exits intersection
180
                        sendBuffer[0] = WINTERSECTIONEXIT;
181
                        sendBuffer[2] = intersectionNum;//Intersection #
182
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
183
                        orb1_set_color(ORANGE);
184
                        
185
                        if(!(sign & CINTERSECTION)){
186
                                if(sign & CHIGHWAYROAD){
187
                                        state = SHIGHWAY;
188
                                }else{
189
                                        state = SROAD;
190
                                }
191
                        }
192
                        break;
193
                case SHIGHWAY:/*On highway*/
194
//                        sign = lineFollow();
195
                        //highway behaviors
196
                                //merging
197
                                //passing
198
                        //read barcode
199
                        break;
200
                default:
201
                        usb_puts("I got stuck in an unknown state! My state is ");
202
                        usb_puti(state);
203
                }
204
        }
205

    
206
}