Colony

#include <stdlib.h>

#include <stdio.h>

#include <string.h>

#include <wireless.h>

#include <lights.h>

#include <dragonfly_lib.h>

#include <xbee.h>

uint8_t group_1_function(void);

uint8_t group_2_function(void);

uint8_t group_3_function(void);

uint8_t group_4_function(void);

/* This function tests out the full functionality of the new wireless library.

 * It must be run on two colony bots simultaneously to properly test all funtions.

*/

int main(int argsc, char* argv[])

{

        orb_init();

        usb_init();

        int status = 0;

        usb_puts("New Wireless Library Test Suite\r\n");

        /*Initialize the wirelss library*/

        usb_puts("Initializing library...\r\n");

        orb1_set_color(BLUE);

        status = wl_init();

        switch(status){

                case 0:

                        usb_puts("Library initialization successful\r\n\r\n");

                        orb1_set_color(GREEN);

                        break;

                case -1:

                        usb_puts("Initialization failed: already initialized\r\n\r\n");

                        orb1_set_color(GREEN);

                        break;

                case -2:

                        usb_puts("Initialization failed: XBEE initialization failed\r\n\r\n");

                        orb1_set_color(RED);

                        break;

                default:

                        usb_puts("Error: Unreconnized status code: ");

                        usb_puti(status);

                        usb_puts("\r\n\r\n");

                        orb1_set_color(RED);

                        break;

        }

        while((status!=0) && 1);

        /*Test all constants defined correctly*/

        status = 0;

        usb_puts("Testing constants...");

        orb2_set_color(GREEN);

        if(GLOBAL!=0){

                usb_puts("\r\nGLOBAL defined as ");

                usb_puti(GLOBAL);

                status++;

        }

        usb_puts(".");

        if(PAN!=1){

                usb_puts("\r\nPAN defined as ");

                usb_puti(PAN);

                status++;

        }

        usb_puts(".");

        if(BROADCAST!=0xFFFF){

                usb_puts("\r\nBROADCAST defined as ");

                usb_puti(BROADCAST);

                status++;

        }

        usb_puts(".");

        if(RELIABLE!=0){

                usb_puts("\r\nRELIABLE defined as ");

                usb_puti(RELIABLE);

                status++;

        }

        usb_puts(".");

        if(FAST!=1){

                usb_puts("\r\nFAST defined as ");

                usb_puti(FAST);

                status++;

        }

        usb_puts(".");

        if(NORMAL_PRIORITY!=0){

                usb_puts("\r\nNORMAL_PRIORITY defined as ");

                usb_puti(NORMAL_PRIORITY);

                status++;

        }

        usb_puts(".");

        if(HIGH_PRIORITY!=1){

                usb_puts("\r\nHIGH_PRIORITY defined as ");

                usb_puti(HIGH_PRIORITY);

                status++;

        }

        usb_puts(".\r\nConstant test complete. There were ");

        usb_puti(status);

        usb_puts(" errors\r\n");

        if(status!=0){

                orb2_set_color(ORANGE);

                delay_ms(500);

        }

        /*Tests function registration*/

        usb_puts("\r\nTesting function registration");

        int registers[8], count;

        status = 0;

        orb2_set_color(GREEN);

        registers[0] = wl_register_handler(1, group_1_function(), 0);

        registers[1] = wl_register_handler(2, group_2_function(), 1);

        registers[2] = wl_register_handler(3, group_3_function(), 0);

        registers[3] = wl_register_handler(4, group_4_function(), 1);

        registers[4] = wl_register_handler(0, group_1_function(), 0);

        registers[5] = wl_register_handler(6, NULL, 0);

        registers[6] = wl_register_handler(30, group_3_function(), 0);

        registers[7] = wl_register_handler(-1, group_4_function(), 0);

        for(count=0; count<=3; count++){

                if(registers[count] != 0){

                        usb_puts("\r\nFunction registration error: attempt ");

                        usb_puti(count);

                        usb_puts(" returned ");

                        usb_puti(registers[count]);

                        usb_puts(" instead of success\r\n");

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

        for(count=4; count<=6; count++){

                if(registers[count] == 0){

                        usb_puts("\r\nFunction registration error: attempt ");

                        usb_puti(count);

                        usb_puts(" succeeded instead of failing\r\n");

                        orb2_set_color(ORANGE);

                        status++;

                }

                if(registers[count] != 0 && registers[count] != -5){

                        usb_puts("\r\nFunction registration error: attempt ");

                        usb_puti(count);

                        usb_puts(" returned ");

                        usb_puti(registers[count]);

                        usb_puts(" instead of -5\r\n");

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

        if(registers[7] == 0){

                usb_puts("\r\nCongrats, you let an array write to index -1\r\n");

                orb2_set_color(RED);

                status++;

        }

        usb_puts("Registration test completed. There were ");

        usb_puti(status);

        usb_puts(" errors\r\n");

        delay_ms(500);

        /*Tests XBee functions*/

        usb_puts("\r\nTesting XBee fuctions...\r\n");

        status = 0;

        orb2_set_color(GREEN);

        unsigned int pan = xbee_get_pan_id();

        if(pan != 0){

                usb_puts("Pan error: defaulted to non-default Pan id :");

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_set_pan_id(1);

        if(pan != 0){

                usb_puts("\r\nPan error: error setting Pan id: ");

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_get_pan_id();

        if(pan != 0){

                usb_puts("\r\nPan error: Pan id reads different than set value 1: ");

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        usb_puts("\r\nXBee tests completed. There were ");

        usb_puti(status);

        usb_puts(" errors.\r\n");

        /*Tests sending in fast mode*/

        usb_puts("\r\nTests sending basic packets in fast mode...\r\n");

        status = 0;

        uint16_t data = xbee_get_address();

        status = wl_send(data, 2, 0, GLOBAL, BROADCAST, FAST);

        usb_puts("Global fast broadcast basic send exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 0, GLOBAL, PAN, FAST);

        usb_puts("\r\nGlobal fast pan send basic exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 0, GLOBAL, BROADCAST, FAST);

        usb_puts("\r\nGlobal fast broadcast basic send exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 0, GLOBAL, PAN, FAST);

        usb_puts("\r\nGlobal fast pan send basic exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 1, GLOBAL, BROADCAST, FAST);

        usb_puts("\r\nGlobal fast broadcast group 1 send exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 2, GLOBAL, PAN, FAST);

        usb_puts("\r\nGlobal fast pan send group 2 exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 1, GLOBAL, BROADCAST, FAST);

        usb_puts("\r\nGlobal fast broadcast group 3 send exit code ");

        usb_puti(status);

        status = wl_send(data, 2, 2, GLOBAL, PAN, FAST);

        usb_puts("\r\nGlobal fast pan send group 4 exit code ");

        usb_puti(status);

        usb_puts("\r\nFast send tests successful\r\n");

        /*Sends packets in fast mode until other robot responds*/

        usb_puts("Sending basic packets until other robot responds");

        status = 0;

        char *packet = 0; /*will contain a 16 bit address, so length always 2*/

//        while(status == 0){

//                wl_send(data, 2, 0, GLOBAL, BROADCAST, FAST);

        //        status = wl_get_basic(packet, 2);

//        }        

        /*Terminates wireless functions*/

        usb_puts("\r\n\r\nTerminating wireless...\r\n");

        status = wl_init();

        switch(status){

                case 0:

                        usb_puts("Wireless termination successful\r\n\r\n");

                        orb1_set_color(BLUE);

                        break;

                case -3:

                        usb_puts("Termination failed: library not initialized\r\n\r\n");

                        orb1_set_color(BLUE);

                        break;

                case -5:

                        usb_puts("Termination failed\r\n\r\n");

                        orb1_set_color(RED);

                        break;

                case -6:

                        usb_puts("Termination failed: function unregistration failed\r\n\r\n");

                        orb1_set_color(ORANGE);

                        break;

                default:

                        usb_puts("Error: Unreconnized status code: ");

                        usb_puti(status);

                        orb1_set_color(RED);

                        break;

        }

        usb_puts("\r\n\r\nWireless Library tests completed");

        while(1){}

}

uint8_t group_1_function(void){

        usb_puts("\r\nFunction 1 called");

        return 0;

}

uint8_t group_2_function(void){

        usb_puts("\r\nFunction 2 called");

        return 0;

}

uint8_t group_3_function(void){

        usb_puts("\r\nFunction 3 called");

        return 0;

}

uint8_t group_4_function(void){

        usb_puts("\r\nFunction 4 called");

        return 0;

}