Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / traffic_navigation / main-new.c @ 1980

History | View | Annotate | Download (13.4 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
#include "../linefollowing/lineDrive.h"
10
#ifndef MAIN_NEW
11

    
12
        static int state, sign, turnDir;
13
        static char sendBuffer[PACKET_LENGTH], queuePrevBot, queueNextBot, id, nextDir, nextPath, intersectionNum, resolvPrevBotID = -3;
14
        unsigned char resolvSeed = 0xC9, resolvDraw = 0, resolvPrevBotDraw = 0;
15
        bool done;
16

    
17
        int wirelessPacketHandle(int state);
18
        void enterIntersection(void);
19
        void sendResolv(bool override);
20
        unsigned char resolvRandomNumberGen();
21

    
22
int main (void) {
23
        
24
        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */
25
        dragonfly_init(ALL_ON);
26
        xbee_init();
27
        encoders_init();
28
        lineDrive_init();
29
        rtc_init(SIXTEENTH_SECOND, NULL);        
30
        wl_basic_init_default();
31
        wl_set_channel(13);
32
        initializeData();
33
        
34
        id = get_robotid();
35
        sign = 0;
36
        orb1_set_color(GREEN);
37
        delay_ms(1000);
38
        orb1_set_color(ORB_OFF);
39
        
40
        //Test code
41
        state = SROAD;
42

    
43
        sendBuffer[1] = id;
44

    
45
        /*
46
        doDrive(200);
47
        turn(DOUBLE, ILEFT);
48
        */
49
        /*
50
        while(1)
51
                doDrive(255);
52
        */
53
        
54
        while (1) {
55
                /*DTM Finite State Machine*/
56
                switch(state){
57
                case SROAD:/*Following a normal road*/
58
                        /* implement other road behaviors?
59
                         *    -tailgating
60
                         */
61
#ifdef ORB_INTERSECTION
62
                        orb1_set_color(WHITE);
63
                        orb2_set_color(ORB_OFF);
64
#endif
65
                        start();
66
                        done = false;
67
                        while(!done){
68
                                sign = doDrive(200);
69
                                switch(sign){
70
                                        case NORMAL:
71
                                        case FINISHED:
72
                                        case LOST:
73
                                        case ERROR:
74
                                                break;
75
                                        default:
76
                                                //we have a barcode!
77
                                                state = SINTERSECTION_ENTER;
78
                                                done = true;
79
                                                break;
80

    
81
                                }
82
                        }
83
                        break;
84
                case SINTERSECTION_ENTER:/*Entering, and in intersection*/
85
                        stop();
86
                        doDrive(0);
87
#ifdef ORB_INTERSECTION
88
                        orb1_set_color(RED);
89
                        orb2_set_color(ORB_OFF);
90
#endif
91

    
92
#ifdef DEBUG_INTERSECTION
93
                        usb_puts("STATE: SINTERSECTION_ENTER\n");
94
#endif
95

    
96
                        /*Intersection queue:
97
                         *Each robot when entering the intersection will check for other robots
98
                         *in the intersection, and insert itself in a queue to go through.
99
                         */
100
                        queuePrevBot = -1; //the bot that will drive before this bot
101
                        queueNextBot = -1; //the bot that will drive after this bot
102
                        resolvPrevBotID = -3; //in the case of a race, the bot that is
103
                                              //to enter the queue before this bot
104
                        resolvPrevBotDraw = 0; //the random priority number that bot has
105
                        resolvDraw = 0; //my random priority number
106
                        
107
                        intersectionNum = getIntersectNum(sign);
108
                        if(intersectionNum == (char) -1){ //invalid
109
                                state = SROAD;
110
                                break;
111
                        }
112
                        turnDir = validateTurn(sign, getTurnType(sign));
113
                        if(turnDir == (char) -1){ //invalid
114
                                state = SROAD;
115
                                break;
116
                        }
117

    
118
                        enterIntersection(); //sends wireless packet for entry
119
                        state = SINTERSECTION_WAIT;
120

    
121
                        rtc_reset(); //reset rtc for timeout wait for reply
122
                        done = false;
123
                        char retried = 0;
124
                        while(rtc_get() < 16 && !done){//waits for a reply, otherwise assumes it is first in queue
125
                                int ret = wirelessPacketHandle(SINTERSECTION_ENTER);
126
                                if(rtc_get() > 12 && !retried){//by now all resolvs should be done from bots that arrived earlier...
127
#ifdef ORB_INTERSECTION
128
                                        orb2_set_color(PURPLE);
129
#endif
130
                                        enterIntersection();
131
                                        retried = 1;
132
                                }
133
                                switch (ret) {
134
                                        case KPLACEDINQUEUE:
135
#ifdef ORB_INTERSECTION
136
                                                orb2_set_color(GREEN);
137
#endif
138
                                                done = true;
139
                                                break;
140
                                        case KFAILEDTOQUEUE:
141
#ifdef DEBUG_INTERSECTION
142
                                                usb_puts("Failed to queue\n");
143
#endif
144
#ifdef ORB_INTERSECTION
145
                                                orb2_set_color(RED);
146
#endif
147
                                                enterIntersection();
148
                                                rtc_reset();
149
                                                break;
150
                                        case KRESOLVINGENTER:
151

    
152
#ifdef ORB_INTERSECTION
153
                                                orb2_set_color(ORANGE);
154
#endif
155
                                                state = SINTERSECTION_ENTER_RESOLV;
156
                                                done = true;
157
                                                break;
158
                                }
159
                        }
160
                        break;
161

    
162
                case SINTERSECTION_ENTER_RESOLV:
163
#ifdef ORB_INTERSECTION
164
                        orb1_set_color(PURPLE);
165
                        orb2_set_color(ORB_OFF);
166
#endif
167
#ifdef DEBUG_INTERSECTION
168
                        usb_puts("STATE: SINTERSECTION_ENTER_RESOLV\n");
169
#endif
170

    
171
                        rtc_reset();
172
                        done = false;
173
                        retried = 0;
174
                        while(rtc_get() < 9 && !done){
175
                                int ret = wirelessPacketHandle(SINTERSECTION_ENTER_RESOLV);
176
                                switch (ret) {
177
                                        case KRESOLVINGENTER:
178
#ifdef ORB_INTERSECTION
179
                                                orb2_set_color(YELLOW);
180
#endif
181
                                                break;
182
                                        case KPLACEDINQUEUE:
183
#ifdef ORB_INTERSECTION
184
                                                orb2_set_color(GREEN);
185
#endif
186
                                                done = true;
187
                                                break;
188
                                        case KFAILEDTOQUEUE:
189
                                                usb_puts("Failed to queue\n");
190
                                                orb2_set_color(RED);
191
                                                enterIntersection();
192
                                                rtc_reset();
193
                                                break;
194
                                }
195
                                //if resolvPrevBotID == -1, this indicates that
196
                                //there was a prevbot before, but it has entered
197
                                //the queue so it's our turn.
198
                                if(!done && resolvPrevBotID == (char) -1 && !retried){
199
                                        enterIntersection();
200
                                        rtc_reset();
201
                                        retried = 1;
202
                                //if resolvPrevBotID == -2, we have been
203
                                //resolving but never have seen a bot with lower
204
                                //priority than us. after the 6/16ths sec
205
                                //timeout, assume we are first.
206
                                } else if(!done && resolvPrevBotID == (char) -2 && rtc_get() > 6){
207
                                        //send a intersection reply to myself to
208
                                        //trigger other bots to enter queue.
209
                                        sendBuffer[0] = WINTERSECTIONREPLY;
210
                                        sendBuffer[2] = intersectionNum;
211
                                        sendBuffer[3] = id;
212
                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
213

    
214
                                        done = true;
215
                                        break;
216
                                }
217
                        }
218
                        state = SINTERSECTION_WAIT;
219
                        break;
220
                case SINTERSECTION_WAIT:/*Waiting in intersection */
221
#ifdef ORB_INTERSECTION
222
                        orb1_set_color(YELLOW);
223
                        orb2_set_color(ORB_OFF);
224
#endif
225
#ifdef DEBUG_INTERSECTION
226
                        usb_puts("STATE: SINTERSECTION_WAIT\n");
227
#endif
228

    
229
                        while(queuePrevBot != (char) -1){
230
                                int ret = wirelessPacketHandle(state);
231
                                switch (ret){
232
                                        case KFIRSTINQUEUE:
233
#ifdef ORB_INTERSECTION
234
                                                orb2_set_color(GREEN);
235
#endif
236
                                                state = SINTERSECTION_DRIVE;
237
                                                break;
238
                                        case KREPLIEDTOENTER:
239
#ifdef ORB_INTERSECTION
240
                                                orb2_set_color(BLUE);
241
#endif
242
                                                break;
243
                                        case KRESOLVINGENTER:
244
#ifdef ORB_INTERSECTION
245
                                                orb2_set_color(ORANGE);
246
#endif
247
                                                break;
248
                                }
249
                        }
250
                        state = SINTERSECTION_DRIVE;
251
                        break;
252
                        
253
                case SINTERSECTION_DRIVE:
254
#ifdef DEBUG_INTERSECTION
255
                        usb_puts("STATE: SINTERSECTION_DRIVE\n");
256
#endif
257
#ifdef ORB_INTERSECTION
258
                        orb1_set_color(GREEN);
259
                        orb2_set_color(ORB_OFF);
260
#endif
261
                        start();
262
                        turn(getIntersectType(sign), turnDir);
263
                        while(doDrive(200) != FINISHED){
264
                        //while(!button2_click()){
265
                                 int ret = wirelessPacketHandle(state);
266
                                 switch (ret){
267
                                         case KREPLIEDTOENTER:
268
#ifdef ORB_INTERSECTION
269
                                                orb2_set_color(BLUE);
270
#endif
271
                                                break;
272
                                         case KRESOLVINGENTER:
273
#ifdef ORB_INTERSECTION
274
                                                 orb2_set_color(ORANGE);
275
#endif
276
                                                break;
277
                                 }
278
                         }
279
                        
280
                        //Exits intersection
281
#ifdef ORB_INTERSECTION
282
                        orb1_set_color(WHITE);
283
#endif
284

    
285
                        sendBuffer[0] = WINTERSECTIONEXIT;
286
                        sendBuffer[2] = intersectionNum;//Intersection #
287
                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
288

    
289
                        //Exits intersection
290
                        /*
291
                        while(1){
292
                                if(button1_click()){
293
                                        start();
294
                                        state = SHIGHWAY;
295
                                        break;
296
                                }
297
                                if(button2_click()){
298
                                        start();
299
                                        state = SROAD;
300
                                        break;
301
                                }
302
                        }*/
303
                        state = SROAD;
304
                        break;
305
                case SHIGHWAY:/*On highway*/
306
#ifdef ORB_INTERSECTION
307
                        orb1_set_color(CYAN);
308
#endif
309
                        while(!button1_click()){
310
                                highwayFSM();
311
                        }
312
                        state = SINTERSECTION_ENTER;
313
                        break;
314
                default:
315
                        usb_puts("I got stuck in an unknown state! My state is ");
316
                        usb_puti(state);
317
                }
318
        }
319

    
320
}
321

    
322
int wirelessPacketHandle(int state){
323
        int dataLength = 0;
324
        unsigned char *packet = NULL;
325
        packet = wl_basic_do_default(&dataLength);
326
        
327
        /* sanity check */
328
        if(dataLength == 0 || packet == NULL) //no packet
329
                return ENOPACKET;
330

    
331
        if(dataLength != PACKET_LENGTH)
332
                return EPACKETLEN;
333

    
334
        usb_puts("Recieved Wireless Packet: ");
335
        for(int i = 0; i < dataLength; i++){
336
                usb_puti(packet[i]);
337
                usb_putc(' ');
338
        }
339
        usb_putc('\n');
340

    
341
        switch (packet[0]){
342
                case WROADENTRY: //[type, bot, road]
343
                        return ENOACTION;
344
                        break;
345
                case WROADEXIT: //[type, bot, road]
346
                        return ENOACTION;
347
                        break;
348
                case WROADSTOP: //[type, bot, road]
349
                        return ENOACTION;
350
                        break;
351
                case WINTERSECTIONENTRY: //[type, bot, intersection, fromDir, toDir]
352
                        if (packet[2] == intersectionNum){
353
                                switch (state){
354
                                        case SINTERSECTION_ENTER:
355
                                                sendResolv(false);
356
                                                resolvPrevBotID = -2;
357
                                                return KRESOLVINGENTER;
358
                                                break;
359
                                        case SINTERSECTION_ENTER_RESOLV:
360
                                                return ENOACTION;
361
                                        case SINTERSECTION_WAIT:
362
                                        case SINTERSECTION_DRIVE:
363
                                                if(queueNextBot == (char) -1){
364
                                                        sendBuffer[0] = WINTERSECTIONREPLY;
365
                                                        sendBuffer[2] = intersectionNum;
366
                                                        sendBuffer[3] = packet[1];
367
                                                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
368
                                                        queueNextBot = packet[1];
369
                                                        return KREPLIEDTOENTER;
370
                                                }
371
                                                break;
372

    
373
                                }
374
                        }
375
                        break;
376
                case WINTERSECTIONREPLY: //[type, fromBot, intersection, toBot]
377
                        if (packet[2] == intersectionNum){
378
                                switch (state){
379
                                        case SINTERSECTION_ENTER:
380
                                        case SINTERSECTION_ENTER_RESOLV:
381
                                                if(packet[3] == id){ //Reply for me
382
                                                        queuePrevBot = packet[1];
383
                                                        return KPLACEDINQUEUE;
384
                                                } else {
385
                                                        if(packet[3] == resolvPrevBotID)
386
                                                                resolvPrevBotID = -1;
387
                                                        return KFAILEDTOQUEUE;
388
                                                }
389
                                                break;
390
                                        default:
391
                                                return ENOACTION;
392
                                }
393
                        }
394
                        break;
395
                case WINTERSECTIONEXIT: //[type, bot, intersection]
396
                        if (packet[2] == intersectionNum){
397
                                switch (state){
398
                                        case SINTERSECTION_WAIT:
399
                                                if(packet[1]==queuePrevBot){
400
                                                        queuePrevBot=-1;
401
                                                        return KFIRSTINQUEUE;
402
                                                }
403
                                }
404
                        }
405
                        break;
406
                case WINTERSECTIONGO: //[type, bot, intersection]
407
                        break;
408
                case WINTERSECTIONPOLICEENTRY: //[?]
409
                        return ENOACTION;
410
                        break;
411
                case WINTERSECTIONRESOLVERACE: //[type, bot, intersection, num]
412
                        //in the case robots draw the same number these will be
413
                        //arbitrated using the sending robot's id, prefering
414
                        //lower ids
415
                        usb_puts("Now in wireless WINTERSECTIONRESOLVERACE handler: resolvPrevBotID: ");
416
                        usb_puti((int) resolvPrevBotID);
417
                        usb_putc('\n');
418
                        if (packet[2] == intersectionNum){
419
                                switch (state){
420
                                        case SINTERSECTION_ENTER:
421
                                        case SINTERSECTION_ENTER_RESOLV:
422
                                                if(resolvPrevBotID == (char) -3){
423
                                                        usb_puts("resolvPrevBotID == -3; sending a resolv packet and setting to -2\n");
424
                                                        sendResolv(false);
425
                                                        resolvPrevBotID = -2;
426
                                                }
427
                                                if((unsigned char) packet[3] == resolvDraw && id > packet[1]){
428
                                                        //other bot drew same number as me,
429
                                                        //and i have a higher id, so it goes first.
430
                                                        usb_puts("bot ");
431
                                                        usb_puti(packet[1]);
432
                                                        usb_puts(" Drew the same number as me and I have a higher id, so it goes first\n");
433
                                                        resolvPrevBotID = packet[1];
434
                                                } else if((unsigned char) packet[3] == resolvPrevBotDraw && resolvPrevBotID > packet[1]){
435
                                                        //other bot drew same number as the bot before me,
436
                                                        //so if it has a higher id than the bot before me,
437
                                                        //it is going to go in between that one and me.
438
                                                        usb_puts("bot ");
439
                                                        usb_puti(packet[1]);
440
                                                        usb_puts(" Drew the same number as the bot before me, and the bot before me has have a higher id, so this goes first\n");
441
                                                        resolvPrevBotID = packet[1];
442
                                                } else if((unsigned char)packet[3] < resolvDraw && (unsigned char)packet[3] > resolvPrevBotDraw){
443
                                                        //found a bot that goes in between the bot before me
444
                                                        //and me, so now it is before me.
445
                                                        usb_puts("bot ");
446
                                                        usb_puti(packet[1]);
447
                                                        usb_puts(" Drew a lower number bot before me, and the bot before me has have a higher id, so this goes first\n");
448
                                                        resolvPrevBotDraw = packet[3];
449
                                                        resolvPrevBotID = packet[1];
450
                                                }
451
                                                return KRESOLVINGENTER;
452
                                                break;
453
                                        case SINTERSECTION_WAIT:
454
                                        case SINTERSECTION_DRIVE:
455
                                                usb_puts("Trying to resolv in non resolv case...queueNextbot = "); usb_puti(queueNextBot); usb_puts("\n");
456
                                                if(queueNextBot == (char) -1){
457
                                                        sendResolv(true);
458
                                                }
459
                                                return KRESOLVINGENTER;
460
                                                break;
461
                                }
462
                                
463
                        }
464
                        break;
465
                case WHIGHWAYENTRY: //[type, bot, highway]
466
                        return ENOACTION;
467
                        break;
468
                case WHIGHWAYREPLY: //[type, fromBot, highway, toBot]
469
                        return ENOACTION;
470
                        break;
471
                case WHIGHWAYEXIT: //[type, bot, highway]
472
                        return ENOACTION;
473
                        break;
474
                case WPINGGLOBAL: //[type, bot]
475
                        return ENOACTION;
476
                        break;
477
                case WPINGBOT: //[type, fromBot, toBot]
478
                        return ENOACTION;
479
                        break;
480
                case WPINGQUEUE: //[type, fromBot, toBot]
481
                        return ENOACTION;
482
                        break;
483
                case WPINGREPLY: //[type, fromBot, toBot]
484
                        return ENOACTION;
485
                        break;
486
                case WCOLLISIONAVOID: //[type, bot, intersection, collision-int] //Note: collision is an int and thus takes two spaces
487
                        return ENOACTION;
488
                        break;
489
                default:
490
                        return ENOACTION;
491
                        break;
492
        }
493
}
494

    
495
unsigned char resolvRandomNumberGen(){
496
        if ((resolvSeed *= (rtc_get())%9) == 0){
497
                return resolvSeed + 1; //0 is a reseved priority value for the last
498
                                 //bot that is already in the queue.
499
        }
500
        return resolvSeed;
501
}
502
void sendResolv(bool override){
503
        if(!override)
504
                resolvDraw = resolvRandomNumberGen();
505
        else
506
                resolvDraw = 0;
507
        sendBuffer[0] = WINTERSECTIONRESOLVERACE;
508
        sendBuffer[2] = intersectionNum;
509
        sendBuffer[3] = resolvDraw;
510
        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
511
}
512
void enterIntersection(void){
513
        //Sends packet announcing its entry to the intersection
514
        sendBuffer[0] = WINTERSECTIONENTRY;
515
        sendBuffer[2] = intersectionNum;//Intersection #
516
        sendBuffer[3] = 0; //getIntersectPos(sign);
517
        sendBuffer[4] = turnDir;
518
        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);
519
}
520
#endif