Colony

/* TODO:

 *  - Document how to call functions.

 *  - Actually Test this stuff. Especially the intersection rotating thing. and the parts marked sketchy.

 *  - Integrate into main.c

 *  - In the intersection queue in main.c, allow bots to skip each other (since this code allows it).

 *    (because some bot at another intersection entrance may be able to go before another bot at another

 *    intersection entrance, but their order in the queue may not allow this.).

*/

#include "collision_avoid.h"

#include "traffic_navigation.h"

#include "../linefollowing/lineDrive.h"

static unsigned int my_path = 0;

static unsigned int first_hop = 0;

static char intersection = 0;

/*           Dir0

 *         [0 ][4 ]

 *Dir1  [5][8 ][9 ][3]   Dir3

 *      [1][11][10][7]

 *         [6 ][2 ]

 *           Dir2

 *

 *            Dir0

 *          [D0E][D0X]

 *Dir1  [D1X][I0][I1][D3E]   Dir3

 *      [D1E][12][13][D3X]

 *          [D2X][D2E]

 *            Dir2

 */

/* Upon entering an intersection, this function should be called. To achieve

 * nonstop motion when possible, it is preferable to call this right after

 * reading the barcode.

 *

 * This function sets up the "my_path" variable with the proper bits representing

 * the path that the bot will travel in the intersection.

 *

 * char intersectionNum: The # of the intersection

 * char fromDir: The direction the bot is entering the intersection

 * char nextDir: The direction the bot is exiting the intersection

 */

void ca_EnterIntersection(char intersectionNum, char fromDir, char nextDir){

        /* Generic turn model condenses all turns into either:

         * - a right turn from dir0 to dir1

         * - a straight from dir0 to dir2

         * - a left turn dir0 to dir3

         * - maybe a U-turn from dir0 to dir0 **UNIMPLEMENTED*

         */

        /* Fill in dir0 */

        my_path = 0;

        my_path = my_path | D0E;

        my_path = my_path | I0;

        char genericNext = nextDir-fromDir;

        if(genericNext < 0){

                genericNext += 4;

        }

        if(genericNext == 1){ //right turn

#ifdef DEBUG_CA

                usb_puts("CA: Setting up Generic Right Turn\n");

#endif

                my_path = my_path | D1X;

        }

        else if(genericNext == 2){ //straight turn

#ifdef DEBUG_CA

                usb_puts("CA: Setting up Generic Straight Turn\n");

#endif

                my_path = my_path | D2X;

                my_path = my_path | I2;

        }

        else{ //left turn

#ifdef DEBUG_CA

                if(genericNext != 3)

                        usb_puts("CA ERROR: Error in generic turn generator (CA_E0)\n");

                usb_puts("CA: Setting up Generic Left Turn\n");

#endif

                my_path = my_path | D3X;

                my_path = my_path | I2;

                my_path = my_path | I3;

        }

        //now rotate turn to match what it's really supposed to be

        for(int i = 0; i < fromDir; i++){

                int new_path = 0;

                char temp = my_path & DAE;

                char shiftoff = temp & 0x8;

                temp = temp << 1;

                temp = temp | (shiftoff >> 3);

                new_path = new_path | temp;

                temp = (my_path & DAX) >> DAXS;

                shiftoff = temp & 0x1;

                temp = temp << 1;

                temp = temp | (shiftoff >> 3);

                new_path = new_path | (temp << DAXS);

                temp = (my_path & DAX) >> IAS;

                shiftoff = temp & 0x1;

                temp = temp << 1;

                temp = temp | (shiftoff >> 3);

                new_path = new_path | (temp << IAS);

                my_path = new_path;

        }

        first_hop = fromDir;

        intersection = intersectionNum;

}

/* Call this function when the bot has left the intersection.

 *

 * All this function does is clear some variables. I guess

 * this is optional but it is nice for debugging. */

void ca_ExitIntersection(void){

        my_path = 0;

        first_hop = -1;

        intersection = -1;

}

/* Don't call this function. This is strictly an internal function to this library.

 *

 * int path: Takes the path to be sent in the packet.

 *

 * This will send a collision avoidance packet with the bot id, the intersection # and path */

void ca_sendPacket(char id, int path){

        if(path & D0E && path & D1E && path & D2E && path & D3E){

#ifdef DEBUG_CA

                usb_puts("CA: No open entry ways to intersection, so not sending CA packet.");

#endif

                return;

        }

        char sendBuffer[PACKET_LENGTH];

        sendBuffer[0] = WCOLLISIONAVOID;

        sendBuffer[1] = id;

        sendBuffer[2] = intersection;

        sendBuffer[3] = path; /* sketchy because path is an int. so will it take both sendBuffer[3] and sendBuffer[4]? */

        wl_basic_send_global_packet(42, sendBuffer, PACKET_LENGTH);

}

/* Call this if the bot is the first in the queue for the intersection and wishes

 * to attempt collision avoidance.

 *

 * char id: the bot's id.

 *

 * What this does is send a collision avoidance packet with this bot's path.

 */

void ca_Initiator(char id){ //args: (This bot's ID)

#ifdef DEBUG_CA

        if(mypath == 0)

                usb_puts("CA ERROR: Someone Forgot to call ca_EnterIntersection. mypath == 0 (CA_E1)\n");

#endif

        ca_sendPacket(id, my_path);

}

/* Call this when the bot gets a collision avoidance packet that *MATCHES*

 * the intersection it is in and the id identifying the sender in the packet

 * matches the bot id that is in line in front. (so the collision avoidance

 * packet sent by the bot last in the queue gets no listener. This is expected.)

 * This function will return whether it is okay for

 * the bot to proceed into the intersection with a true or false.

 *

 * char id: the bot's id

 * char recieve[]: the wireless packet recieved.

 *

 * What this does is check if the path that the bot needs to travel is clear by

 * comparing my_path and the path in the packet.

 * If it is clear, the bot will add my_path to the path it got in the wireless

 * packet and then send it. Then the function will return true.

 * Otherwise, the bot will only fill in the entrance bit in the function it

 * recieved in the wireless packet to indicate it is sitting at the entrance

 * to the intersection so that bots behind it won't try to enter the intersection

 * and crash into the bot at the front of the line for that intersection.

 */

bool ca_Process(char id, char recieve[]){ //args: (This bot's ID, a pointer to the packet recieved)

        bool result = false;

#ifdef DEBUG_CA

        if(recieve[0] != WCOLLISIONAVOID)

                usb_puts("CA ERROR: You didn't call ca_Process with a WCOLLISIONAVOID type packet (CA_E2)\n");

        if(recieve[1] < 0)

                usb_puts("CA ERROR: Recieved Packet indicates sending bot has a negative ID. (CA_E3)\n");

        if(recieve[2] != intersection)

                usb_puts("CA ERROR: Recieved Packet's intersection doesn't match the intersection CA is operating on in this Bot.. (CA_E4)\n");

#endif

        if((*(recieve+3) & my_path) != 0){ /* Sketchy Code because recieve is made of chars and my_path is an int */

#ifdef DEBUG_CA

                usb_puts("CA: My Path is occupied through intersection ");

                usb_puti(intersection);

                usb_putc('\n');

#endif

                result = false;

                ca_sendPacket(id, 1 << first_hop | *(recieve+3)); /* Sketchy Code because recieve is made of chars and ca_send_packet expects an int. we need the whole int to make it through*/

        }

        else{

#ifdef DEBUG_CA

                usb_puts("CA: My Path is clear through intersection ");

                usb_puti(intersection);

                usb_putc('\n');

#endif

                result = true;

                ca_sendPacket(id, my_path | *(recieve+3)); /* Sketchy Code because recieve is made of chars and ca_send_packet expects an int.  we need the whole int to make it through. Also, my_path is an int.*/

        }

        return result;

}