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);

  uint16_t pan = xbee_get_pan();

  if(pan != 0){

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

    usb_puti(pan);

    orb2_set_color(ORANGE);

    status++;

  }

  status = xbee_set_pan(0x2B7D);

  if(status != 0){

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

    usb_puth16(pan);

    orb2_set_color(ORANGE);

    status++;

  }

  pan = xbee_get_pan();

  if(pan != 0x2B7D){

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

    usb_puth16(pan);

    orb2_set_color(ORANGE);

    status++;

  }

  status = xbee_set_pan(0);

  if(status != 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 0: ");

    usb_puti(pan);

    orb2_set_color(ORANGE);

    status++;

  }

  int chanCount;

  for(chanCount = 0x0B; chanCount <= 0x1A; chanCount++){

    xbee_set_channel(0x10);

    WL_DEBUG_PRINT_P("\r\nChannel: ");

    WL_DEBUG_PRINT_HEX(xbee_get_channel());

  }

  uint16_t data = xbee_get_address();

  uint8_t sendBuffer[2];

  uint16_t address = 0;

  sendBuffer[0] = (data& 0xFF00) >> 8;

  sendBuffer[1] = (data& 0x00FF);

        WL_DEBUG_PRINT_P("\r\n\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;

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

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

        usb_puti(status);

  char bigbuf[10];

  int8_t ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        usb_puti(status);

  ret = wl_get(bigbuf,10);

  if (ret < 0) {

    WL_DEBUG_PRINT_P("error in wl_get:");

    WL_DEBUG_PRINT_INT(ret);

    WL_DEBUG_PRINT_P("\r\n");

  } else {

    WL_DEBUG_PRINT_P("get returned:");

    usb_puth16(bigbuf);

  }

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

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

        status = 0;

  int len = 0;

        uint8_t buffer[10];

//        uint8_t sendBuffer[2];

        //uint16_t address = 0;

        orb2_set_color(BLUE);

        while(1){

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

                usb_puti(data);

                usb_puti(xbee_get_address());

                sendBuffer[0] = (data& 0xFF00) >> 8;

                sendBuffer[1] = (data& 0x00FF);

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

                delay_ms(1000);

                len = wl_get(buffer, 10);

                if(len != 0){

      address = (buffer[0] << 8) + buffer[1];

                        orb2_set_color(GREEN);

                        WL_DEBUG_PRINT_P("\r\nLength: ");

                        WL_DEBUG_PRINT_INT(len);

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

                        usb_puth16(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_terminate();

        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++){

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

                usb_puti(wl_send_basic(&count, 1));

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }        

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

                WL_DEBUG_PRINT_P("\r\nSend number:");

                WL_DEBUG_PRINT_INT(count);

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

                delay_ms(300);

        }

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

        }

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

                usb_puti(count);

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

                delay_ms(1000);

        }

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

                usb_puti(count);

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

                delay_ms(1000);

        }

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

                usb_puti(count);

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

                delay_ms(1000);

        }

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

                usb_puti(count);

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

                delay_ms(1000);

        }

        WL_DEBUG_PRINT_P("ACK Errors: ");

        usb_puti(wl_ack_error());

        wl_ack_reset();

        usb_puti(wl_ack_error());

        for(count = 65; count < 70; 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 = 8;

        char* data[8];

        while(1){

                result = wl_get(data, length);

                if(result){

                        WL_DEBUG_PRINT_P("\r\nData length: ");

                        WL_DEBUG_PRINT_INT(result);

                        WL_DEBUG_PRINT_P("\r\nData: ");

                        WL_DEBUG_PRINT_HEX(*data);

                }else{

                        delay_ms(100);

                }

        }

}

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. Length: ");

        WL_DEBUG_PRINT_INT(length);

        WL_DEBUG_PRINT_P(" Soucre: ");

        WL_DEBUG_PRINT_HEX(source);

        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;

}