Colony

#include <wl_defs.h>

#include <string.h>

#include <wireless.h>

#include <lights.h>

#include <dragonfly_lib.h>

#include <xbee.h>

void group_1_function(uint8_t* data,uint8_t length,uint8_t source);

void group_2_function(uint8_t* data,uint8_t length,uint8_t source);

void group_3_function(uint8_t* data,uint8_t length,uint8_t source);

void group_4_function(uint8_t* data,uint8_t length,uint8_t source);

void test_function_1(void);

void test_sending();

void test_receiving();

uint16_t friendAddress = 0;

/* 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.

*/

void testwireless2(void)

{

        orb_init();

        usb_init();

        int status = 0;

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

        /*Initialize the wirelss library*/

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

        orb1_set_color(BLUE);

        status = wl_init();

        switch(status){

                case 0:

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

                        orb1_set_color(GREEN);

                        break;

                case -1:

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

                        orb1_set_color(GREEN);

                        break;

                case -2:

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

                        orb1_set_color(RED);

                        break;

                default:

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

                        usb_puti(status);

                        WL_DEBUG_PRINT_P("\r\n\r\n");

                        orb1_set_color(RED);

                        break;

        }

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

        /*Test all constants defined correctly*/

        status = 0;

        WL_DEBUG_PRINT_P("Testing constants...");

        orb2_set_color(GREEN);

        if(GLOBAL!=0){

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

                usb_puti(GLOBAL);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(PAN!=1){

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

                usb_puti(PAN);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(BROADCAST!=0xFFFF){

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

                usb_puti(BROADCAST);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(RELIABLE!=0){

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

                usb_puti(RELIABLE);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(FAST!=1){

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

                usb_puti(FAST);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(NORMAL_PRIORITY!=0){

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

                usb_puti(NORMAL_PRIORITY);

                status++;

        }

        WL_DEBUG_PRINT_P(".");

        if(HIGH_PRIORITY!=1){

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

                usb_puti(HIGH_PRIORITY);

                status++;

        }

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

        usb_puti(status);

        WL_DEBUG_PRINT_P(" errors\r\n");

        if(status!=0){

                orb2_set_color(ORANGE);

                delay_ms(500);

        }

        /*Tests function registration*/

        WL_DEBUG_PRINT_P("\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){

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

                        usb_puti(count);

                        WL_DEBUG_PRINT_P(" returned ");

                        usb_puti(registers[count]);

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

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

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

                if(registers[count] == 0){

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

                        usb_puti(count);

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

                        orb2_set_color(ORANGE);

                        status++;

                }

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

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

                        usb_puti(count);

                        WL_DEBUG_PRINT_P(" returned ");

                        usb_puti(registers[count]);

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

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

        if(registers[7] == 0){

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

                orb2_set_color(RED);

                status++;

        }

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

        usb_puti(status);

        WL_DEBUG_PRINT_P(" errors\r\n");

        delay_ms(500);

        /*Tests XBee functions*/

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

        status = 0;

        orb2_set_color(GREEN);

        unsigned int pan = xbee_get_pan();

        if(pan != 0){

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

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_set_pan(1);

        if(pan != 0){

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

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_get_pan();

        if(pan != 0){

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

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

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

        usb_puti(status);

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

        /*Tests sending in fast mode*/

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

        status = 0;

        uint16_t data = xbee_get_address();

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        usb_puti(status);

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

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

        status = 0;

        char address[2]; /*will contain a 16 bit address, so length always 2*/

        *address = 0;

        orb2_set_color(BLUE);

        while(status <= 0){

                wl_send((uint8_t*)&data, 2, 0, GLOBAL, BROADCAST, FAST);

                wl_get_basic(address, 2);

                if(address != 0){

                        orb2_set_color(GREEN);

                        WL_DEBUG_PRINT_P("\r\nFriend address: ");

                        usb_puti((uint16_t)address);

                        friendAddress = *address;

                }

                if(button1_read){

                        test_sending();

                        break;

                }

                if(button2_read){

                        test_receiving();

                        break;

                }

        }

        /*Terminates wireless functions*/

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

        status = wl_init();

        switch(status){

                case 0:

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

                        orb1_set_color(BLUE);

                        break;

                case -3:

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

                        orb1_set_color(BLUE);

                        break;

                case -5:

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

                        orb1_set_color(RED);

                        break;

                case -6:

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

                        orb1_set_color(ORANGE);

                        break;

                default:

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

                        usb_puti(status);

                        orb1_set_color(RED);

                        break;

        }

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

        while(1){}

}

void test_sending(){

        uint8_t count;

        //Testing all wrappers for FAST mode

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

                usb_puti(count);

                usb_puti(wl_send_basic(&count, 1));

                delay_ms(count * 10);

        }

        for(count = 5; count < 10; count++){

                usb_puti(count);

                usb_puti(wl_send_global(&count, 1, 0));

                delay_ms(count * 10);

        }        

        for(count = 10; count < 15; count++){

                usb_puti(count);

                usb_puti(wl_send_pan(&count, 1, xbee_get_pan()));

                delay_ms(count * 10);

        }

        for(count = 15; count < 20; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 0, friendAddress, FAST));

                delay_ms(count * 10);

        }

        for(count = 20; count < 25; count++){

                usb_puti(count);

                usb_puti(wl_send_global(&count, 1, 1));

                delay_ms(count * 10);

        }

        for(count = 25; count < 30; count++){

                usb_puti(count);

                usb_puti(wl_send_global(&count, 1, 2));

                delay_ms(count * 10);

        }

        for(count = 30; count < 35; count++){

                usb_puti(count);

                usb_puti(wl_send_global(&count, 1, 3));

                delay_ms(count * 10);

        }

        for(count = 35; count < 40; count++){

                usb_puti(count);

                usb_puti(wl_send_global(&count, 1, 4));

                delay_ms(count * 10);

        }

        //FAST mode tests completed. Now testing ACK system

        for(count = 40; count < 45; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 0, friendAddress, RELIABLE));

                delay_ms(count * 10);

        }

        for(count = 45; count < 50; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 1, friendAddress, RELIABLE));

                delay_ms(count * 10);

        }

        for(count = 50; count < 55; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 2, friendAddress, RELIABLE));

                delay_ms(count * 10);

        }

        for(count = 55; count < 60; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 3, friendAddress, RELIABLE));

                delay_ms(count * 10);

        }

        for(count = 60; count < 65; count++){

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 4, friendAddress, RELIABLE));

                delay_ms(count * 10);

        }

        WL_DEBUG_PRINT_P("ACK Errors: ");

        usb_puti(wl_ack_error());

        wl_ack_reset();

        usb_puti(wl_ack_error());

        for(count = 40; count < 45; count++){//changed friendAddress, all should fail

                usb_puti(count);

                usb_puti(wl_send_robot(&count, 1, 0, friendAddress + 1, RELIABLE));

                delay_ms(count * 10);

        }

        WL_DEBUG_PRINT_P("ACK Errors: ");

        usb_puti(wl_ack_error());

}

void test_receiving(){

        int8_t result, length;

        char* data[8];

        while(1){

                result = wl_get(data, length);

                if(result){

                        usb_puti(*data);

                }

        }

}

void group_1_function(uint8_t* data,uint8_t length,uint8_t source){

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

        return 0;

}

void group_2_function(uint8_t* data,uint8_t length,uint8_t source){

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

        return 0;

}

void group_3_function(uint8_t* data,uint8_t length,uint8_t source){

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

        return 0;

}

void group_4_function(uint8_t* data,uint8_t length,uint8_t source){

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

        return 0;

}

//The first robot to receive the address of another robot will trigger this function.

void test_function_1(void){

        uint16_t myAddress = xbee_get_address();

        usb_puti(myAddress);

        return 0;

}

//Cheat Functions

//These will run instead of the real functions while David fixes the library