Colony

#include <string.h>

#include <wireless.h>

#include <lights.h>

#include <dragonfly_lib.h>

#include <xbee.h>

#include <avr/pgmspace.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);

uint16_t friendAddress = 0;

char debug_output1[] PROGMEM =  "New Wireless Library Test Suite\r\n";

char debug_output2[] PROGMEM =  "Initializing library...\r\n";

char debug_output3[] PROGMEM =  "Library initialization successful\r\n\r\n";

char debug_output4[] PROGMEM =  "Initialization failed: already initialized\r\n\r\n";

char debug_output5[] PROGMEM =  "Initialization failed: XBEE initialization failed\r\n\r\n";

char debug_output6[] PROGMEM =  "Error: Unreconnized status code: ";

char debug_output7[] PROGMEM =  "\r\n\r\n";

char debug_output8[] PROGMEM =  "Testing constants...";

char debug_output9[] PROGMEM =  "\r\nGLOBAL defined as ";

char debug_output10[] PROGMEM = "\r\nPAN defined as ";

char debug_output11[] PROGMEM = "\r\nBROADCAST defined as ";

char debug_output12[] PROGMEM = "\r\nRELIABLE defined as ";

char debug_output13[] PROGMEM = "\r\nFAST defined as ";

char debug_output14[] PROGMEM = "\r\nNORMAL_PRIORITY defined as ";

char debug_output15[] PROGMEM = "\r\nHIGH_PRIORITY defined as ";

char debug_output16[] PROGMEM = ".\r\nConstant test complete. There were ";

char debug_output17[] PROGMEM = " errors\r\n";

char debug_output18[] PROGMEM = "\r\nTesting function registration";

char debug_output19[] PROGMEM = "\r\nFunction registration error: attempt ";

char debug_output20[] PROGMEM = " returned ";

char debug_output21[] PROGMEM = " instead of success\r\n";

char debug_output22[] PROGMEM = "\r\nFunction registration error: attempt ";

char debug_output23[] PROGMEM = " succeeded instead of failing\r\n";

char debug_output24[] PROGMEM = "\r\nFunction registration error: attempt ";

char debug_output25[] PROGMEM = " instead of -5\r\n";

char debug_output26[] PROGMEM = "\r\nCongrats, you let an array write to index -1\r\n";

char debug_output27[] PROGMEM = "Registration test completed. There were ";

char debug_output28[] PROGMEM = " errors\r\n";

char debug_output29[] PROGMEM = "\r\nTesting XBee fuctions...\r\n";

char debug_output30[] PROGMEM = "Pan error: defaulted to non-default Pan id :";

char debug_output31[] PROGMEM = "\r\nPan error: error setting Pan id: ";

char debug_output32[] PROGMEM = "\r\nPan error: Pan id reads different than set value 1: ";

char debug_output33[] PROGMEM = "\r\nXBee tests completed. There were ";

char debug_output34[] PROGMEM = " errors.\r\n";

char debug_output35[] PROGMEM = "\r\nTests sending basic packets in fast mode...\r\n";

char debug_output36[] PROGMEM = "Global fast broadcast basic send exit code ";

char debug_output37[] PROGMEM = "\r\nGlobal fast pan send basic exit code ";

char debug_output38[] PROGMEM = "\r\nGlobal fast broadcast basic send exit code ";

char debug_output39[] PROGMEM = "\r\nGlobal fast pan send basic exit code ";

char debug_output40[] PROGMEM = "\r\nGlobal fast broadcast group 1 send exit code ";

char debug_output41[] PROGMEM = "\r\nGlobal fast pan send group 2 exit code ";

char debug_output42[] PROGMEM = "\r\nGlobal fast broadcast group 3 send exit code ";

char debug_output43[] PROGMEM = "\r\nGlobal fast pan send group 4 exit code ";

char debug_output44[] PROGMEM = "\r\nFast send tests successful\r\n";

char debug_output45[] PROGMEM = "Sending basic packets until other robot responds";

char debug_output46[] PROGMEM = "\r\nReceive error code ";

char debug_output47[] PROGMEM = "\r\nReceived packet from robot: ";

char debug_output48[] PROGMEM = "\r\n\r\nTerminating wireless...\r\n";

char debug_output49[] PROGMEM = "Wireless termination successful\r\n\r\n";

char debug_output50[] PROGMEM = "Termination failed: library not initialized\r\n\r\n";

char debug_output51[] PROGMEM = "Termination failed\r\n\r\n";

char debug_output52[] PROGMEM = "Termination failed: function unregistration failed\r\n\r\n";

char debug_output53[] PROGMEM = "Error: Unreconnized status code: ";

char debug_output54[] PROGMEM = "\r\n\r\nWireless Library tests completed";

char debug_output55[] PROGMEM = "\r\nFunction 1 called";

char debug_output56[] PROGMEM = "\r\nFunction 2 called";

char debug_output57[] PROGMEM = "\r\nFunction 3 called";

char debug_output58[] PROGMEM = "\r\nFunction 4 called";

/* 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();

        uint8_t status = 0;

        usb_puts((PGM_P)debug_output1);

        /*Initialize the wirelss library*/

        usb_puts(debug_output2);

        orb1_set_color(BLUE);

        status = wl_init();

        switch(status){

                case 0:

                        usb_puts((PGM_P)debug_output3);

                        orb1_set_color(GREEN);

                        break;

                case -1:

                        usb_puts((PGM_P)debug_output4);

                        orb1_set_color(GREEN);

                        break;

                case -2:

                        usb_puts((PGM_P)debug_output5);

                        orb1_set_color(RED);

                        break;

                default:

                        usb_puts((PGM_P)debug_output6);

                        usb_puti(status);

                        usb_puts((PGM_P)debug_output7);

                        orb1_set_color(RED);

                        break;

        }

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

        /*Test all constants defined correctly*/

        status = 0;

        usb_puts((PGM_P)debug_output8);

        orb2_set_color(GREEN);

        if(GLOBAL!=0){

                usb_puts((PGM_P)debug_output9);

                usb_puti(GLOBAL);

                status++;

        }

        usb_puts(".");

        if(PAN!=1){

                usb_puts((PGM_P)debug_output10);

                usb_puti(PAN);

                status++;

        }

        usb_puts(".");

        if(BROADCAST!=0xFFFF){

                usb_puts((PGM_P)debug_output11);

                usb_puti(BROADCAST);

                status++;

        }

        usb_puts(".");

        if(RELIABLE!=0){

                usb_puts((PGM_P)debug_output12);

                usb_puti(RELIABLE);

                status++;

        }

        usb_puts(".");

        if(FAST!=1){

                usb_puts((PGM_P)debug_output13);

                usb_puti(FAST);

                status++;

        }

        usb_puts(".");

        if(NORMAL_PRIORITY!=0){

                usb_puts((PGM_P)debug_output14);

                usb_puti(NORMAL_PRIORITY);

                status++;

        }

        usb_puts(".");

        if(HIGH_PRIORITY!=1){

                usb_puts((PGM_P)debug_output15);

                usb_puti(HIGH_PRIORITY);

                status++;

        }

        usb_puts((PGM_P)debug_output16);

        usb_puti(status);

        usb_puts((PGM_P)debug_output17);

        if(status!=0){

                orb2_set_color(ORANGE);

                delay_ms(500);

        }

        /*Tests function registration*/

        usb_puts((PGM_P)debug_output18);

        int8_t 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((PGM_P)debug_output19);

                        usb_puti(count);

                        usb_puts((PGM_P)debug_output20);

                        usb_puti(registers[count]);

                        usb_puts((PGM_P)debug_output21);

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

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

                if(registers[count] == 0){

                        usb_puts((PGM_P)debug_output22);

                        usb_puti(count);

                        usb_puts((PGM_P)debug_output23);

                        orb2_set_color(ORANGE);

                        status++;

                }

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

                        usb_puts((PGM_P)debug_output24);

                        usb_puti(count);

                        usb_puts((PGM_P)debug_output20);

                        usb_puti(registers[count]);

                        usb_puts((PGM_P)debug_output25);

                        orb2_set_color(ORANGE);

                        status++;

                }

        }

        if(registers[7] == 0){

                usb_puts((PGM_P)debug_output26);

                orb2_set_color(RED);

                status++;

        }

        usb_puts((PGM_P)debug_output27);

        usb_puti(status);

        usb_puts((PGM_P)debug_output28);

        delay_ms(500);

        /*Tests XBee functions*/

        usb_puts((PGM_P)debug_output29);

        status = 0;

        orb2_set_color(GREEN);

        unsigned int pan = xbee_get_pan();

        if(pan != 0){

                usb_puts((PGM_P)debug_output30);

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_set_pan(1);

        if(pan != 0){

                usb_puts((PGM_P)debug_output31);

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        pan = xbee_get_pan();

        if(pan != 0){

                usb_puts((PGM_P)debug_output32);

                usb_puti(pan);

                orb2_set_color(ORANGE);

                status++;

        }

        usb_puts((PGM_P)debug_output33);

        usb_puti(status);

        usb_puts((PGM_P)debug_output34);

        /*Tests sending in fast mode*/

        usb_puts((PGM_P)debug_output35);

        status = 0;

        uint16_t data = xbee_get_address();

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

        usb_puts((PGM_P)debug_output36);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output37);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output38);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output39);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output40);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output41);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output42);

        usb_puti(status);

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

        usb_puts((PGM_P)debug_output43);

        usb_puti(status);

        usb_puts((PGM_P)debug_output44);

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

        usb_puts((PGM_P)debug_output45);

        status = 0;

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

        while(status <= 0){

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

                status = wl_get_basic(address, 2);

                if(status < 0){

                        usb_puts((PGM_P)debug_output46);

                        usb_puti(status);

                        orb2_set_color(ORANGE);

                }                        

        }

        usb_puts((PGM_P)debug_output47);

        usb_puti((uint16_t)address);

        friendAddress = *address;

        /*Terminates wireless functions*/

        usb_puts((PGM_P)debug_output48);

        status = wl_init();

        switch(status){

                case 0:

                        usb_puts((PGM_P)debug_output49);

                        orb1_set_color(BLUE);

                        break;

                case -3:

                        usb_puts((PGM_P)debug_output50);

                        orb1_set_color(BLUE);

                        break;

                case -5:

                        usb_puts((PGM_P)debug_output51);

                        orb1_set_color(RED);

                        break;

                case -6:

                        usb_puts((PGM_P)debug_output52);

                        orb1_set_color(ORANGE);

                        break;

                default:

                        usb_puts((PGM_P)debug_output53);

                        usb_puti(status);

                        orb1_set_color(RED);

                        break;

        }

        usb_puts((PGM_P)debug_output54);

        while(1){}

}

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

        usb_puts((PGM_P)debug_output55);

        return 0;

}

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

        usb_puts((PGM_P)debug_output56);

        return 0;

}

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

        usb_puts((PGM_P)debug_output57);

        return 0;

}

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

        usb_puts((PGM_P)debug_output58);

        return 0;

}

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

uint8_t 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