Colony

/*

 * main.c for Traffic Navigation

 * Runs the highest level behavior for the Dynamic Traffic Navigation (DTM) SURG

 *

 * Author: Benjamin Wasserman, Colony Project, CMU Robotics Club

 */

#include <dragonfly_lib.h>

#include <wl_basic.h>

#include "lineFollow.h"

/*States*/

#define SROAD 0

#define SINTERSECTION 10

#define SHIGHWAY 20

/*Sign Codes

 * bitwise OR labels to create a barcode or read one

 * There should be macros to extract these probably

 * The bits will be stored in some variable (char or short)

 * Bits if road: ? ? ? NAME NAME NAME TYPE CROAD

 * Bits if intersection: ? ? ? ? DIR DIR #WAYS CINTERSECTION

 */

#define CROAD 0x0 //0b

#define CINTERSECTION 0x1 //1b

#define CNORMALROAD 0x0 //00b

#define CHIGHWAYROAD 0x2 //10b

#define C4WAY 0x0 //00b

#define C3WAY 0x2 //10b

#define CNORTH 0x0 //0000b

#define CEAST 0x4 //0100b

#define CSOUTH 0x8 //1000b

#define CWEST 0x12 //1100b

/*Wireless Packet Types

 * The first byte of any wireless packet should be one of these types.

 * Each type will have its own structure

 * The second byte should be the id of the bot sending the packet

 * The third byte should be the number of the intersection or road that

 *   the packet pertains to

 */

#define PACKET_LENGTH 5

#define WROADENTRY 0 //[type, bot, road]

#define WROADREPLY 1 //[type, fromBot, road, toBot]

#define WROADEXIT 2 //[type, bot, road]

#define WROADSTOP 3 //[type, bot, road]

#define WINTERSECTIONENTRY 10 //[type, bot, intersection, fromDir, toDir]

#define WINTERSECTIONREPLY 11 //[type, fromBot, intersection, toBot]

#define WINTERSECTIONEXIT 12 //[type, bot, intersection]

#define WINTERSECTIONGO 13 //[type, bot, intersection]

#define WINTERSECTIONPOLICEENTRY 14

#define WHIGHWAYENTRY 20 //[type, bot, highway]

#define WHIGHWAYREPLY 21 //[type, fromBot, highway, toBot]

#define WHIGHWAYEXIT 22 //[type, bot, highway]

#define WPINGGLOBAL 30 //[type, bot]

#define WPINGBOT 31 //[type, fromBot, toBot]

#define WPINGQUEUE 32 //[type, bot, queue]

#define WPINGREPLY 33 //[type, fromBot, toBot]

int main (void) {

        int state, sign, dataLength;

        char sendBuffer[PACKET_LENGTH], prevBot, nextBot, id, *packet, nextDir, nextPath, intersectionNum;

        /* Initialize the dragonfly boards, the xbee, encoders, lineFollowing */

        dragonfly_init(ALL_ON);

        xbee_init();

        encoders_init();

        lineFollow_init();

        rtc_init(SIXTEENTH_SECOND, NULL);        

        wl_basic_init_default();

        wl_set_channel(13);

        id = get_robotid();

        sign = 0;

        //Test code

        state = SROAD;

        sendBuffer[1] = id;

        while (1) {

                /*DTM Finite State Machine*/

                switch(state){

                case SROAD:/*Following a normal road*/

//                        sign = lineFollow();

                        //other road behaviors

                                //tailgating?

                        //read barcode

                        if((sign & CINTERSECTION) || button1_click()){

                                state = SINTERSECTION;

                        }

                        break;

                case SINTERSECTION:/*Entering, and in intersection*/

                        //Intersection behaviors

                                //queueing

                                //no-stop?

                        //check wireless

                        intersectionNum = 0;

                        /*Intersection queue:

                         *Each robot when entering the intersection will check for other robots

                         *in the intersection, and insert itself in a queue to go through.

                         */

                        prevBot = 0;

                        nextBot = 0;

                        //Sends packet announcing its entry to the intersection

                        sendBuffer[0] = WINTERSECTIONENTRY;

                        sendBuffer[2] = intersectionNum;//Intersection #

                        sendBuffer[3] = 0;//From Direction

                        sendBuffer[4] = 2;//To Direction

                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                        orb1_set_color(BLUE);

                        rtc_reset();

                        while(rtc_get() < 8){//waits for a reply, otherwise assumes it is first in queue

                                packet = wl_basic_do_default(&dataLength);

                                if(dataLength==PACKET_LENGTH && packet[0]==WINTERSECTIONREPLY && packet[2]==intersectionNum/*Intersection Num*/){

                                        if(packet[3]==id){//Reply for me

                                                prevBot = packet[1];

                                                orb2_set_color(GREEN);

                                                break;

                                        }else{//Someone else got here first, try again

                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                                                rtc_reset();

                                        }

                                }

                        }

                        orb1_set_color(PURPLE);

                        //waits for its turn

                        while(prevBot != 0){

                                packet = wl_basic_do_default(&dataLength);                

                                if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){

                                        if(packet[1]==prevBot && (packet[0]==WINTERSECTIONGO || packet[0]==WINTERSECTIONEXIT)){

                                                prevBot = 0;

                                                orb2_set_color(PURPLE);

                                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){

                                                sendBuffer[0] = WINTERSECTIONREPLY;

                                                sendBuffer[2] = intersectionNum;//Intersection #

                                                sendBuffer[3] = packet[1];

                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                                                nextBot = packet[1];

                                                nextDir = packet[3];

                                                nextPath = packet[4];

                                                orb2_set_color(BLUE);

                                        }

                                }

                        }

                        orb1_set_color(RED);

                        //Drives through intersection

                        //Code to choose path through intersection goes in this while loop

                        //But here's the code to handle wireless while in the intersection...

                        while(!button1_click()/*replace with variable to keep track of if in intersection*/){

                                packet = wl_basic_do_default(&dataLength);                

                                if(dataLength==PACKET_LENGTH && packet[2]==intersectionNum/*Intersection Num*/){

                                        if(packet[0]==WINTERSECTIONEXIT){

                                                prevBot = 0;

                                                orb2_set_color(RED);

                                        }else if(packet[0]==WINTERSECTIONENTRY && nextBot==0){

                                                sendBuffer[0] = WINTERSECTIONREPLY;

                                                sendBuffer[2] = intersectionNum;//Intersection #

                                                sendBuffer[3] = packet[1];

                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                                                nextBot = packet[1];

                                                nextDir = packet[3];

                                                nextPath = packet[4];

                                                orb2_set_color(YELLOW);

                                        }

                                }

                                if(prevBot==0 && nextBot!=0){//let 2 bots go through at same time

                                        /*if(bots paths won't collide){

                                                sendBuffer[0] = WINTERSECTIONGO;

                                                sendBuffer[2] = 0;//Intersection #

                                                wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                                        */

                                }

                        }

                        //Exits intersection

                        sendBuffer[0] = WINTERSECTIONEXIT;

                        sendBuffer[2] = intersectionNum;//Intersection #

                        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

                        orb1_set_color(ORANGE);

                        if(!(sign & CINTERSECTION)){

                                if(sign & CHIGHWAYROAD){

                                        state = SHIGHWAY;

                                }else{

                                        state = SROAD;

                                }

                        }

                        break;

                case SHIGHWAY:/*On highway*/

//                        sign = lineFollow();

                        //highway behaviors

                                //merging

                                //passing

                        //read barcode

                        break;

                default:

                        usb_puts("I got stuck in an unknown state! My state is ");

                        usb_puti(state);

                }

        }

}