Colony

/**

 * Instructions:

 *  - wait for both robots to show ORB 1 RED and ORB 2 GREEN

 *  - press BUTTON 1 on robot to send first (ROBOT 1) - orbs should turn PURPLE

 *  - press BUTTON 2 on robot to receive first (ROBOT 2) - orbs should turn ORANGE

 *  - wait at least a second, then press BUTTON 1 on both robots SIMULTANEOUSLY

 *  - ROBOT 2 orbs should cycle ORANGE to RED to BLUE

 *  - then ROBOT 1 orbs should cycle PURPLE to RED to BLUE

 *  - then orbs on both robots should turn GREEN

 *  - test will reset to ORB 1 RED and ORB 2 CYAN

 *  - test should take ~7 seconds

 *

 * Depends on:

 *  - Orbs (Lights)

 *  - Serial (for debugging)

 **/

#include <dragonfly_lib.h>

#include <wl_basic.h>

unsigned char* packet;

int testwireless(void) {

    int count = 0;

    int is_sender = 0;

    wl_basic_init_default();

    wl_set_channel(0x0E);

    orb1_set_color(RED);

    orb2_set_color(GREEN);

    delay_ms(500);

    while (count < 1) {

        if (button1_click()) {

            is_sender = 1;

            orb_set_color(PURPLE);

            count++;

        }

        if (button2_click()) {

            is_sender = 0;

            orb_set_color(ORANGE);

            count++;

        }

    }

    count = 0;

    delay_ms(1000);

    button1_wait();

    delay_ms(500);

    if (is_sender == 1) {

        delay_ms(1000);

        wl_basic_send_global_packet(1, "r", 2);

        usb_puts("sent RED packet\r\n");

        delay_ms(1000);

        wl_basic_send_global_packet(1, "b", 2);

        usb_puts("sent BLUE packet\r\n");

        while(count < 2) {

            packet = wl_basic_do_default();

            if (packet != 0) {

                if (current_packet.length > 0) {

                    if (packet[0] == 'r') {

                        orb_set_color(RED);

                        usb_puts("received RED packet\r\n");

                        count++;

                    }

                    else if (packet[0] == 'b') {

                        orb_set_color(BLUE);

                        usb_puts("received BLUE packet\r\n");

                        count++;

                    }

                }

            }

        }

    }

    else {

        while(count < 2) {

            packet = wl_basic_do_default();

            if (packet != 0) {

                if (current_packet.length > 0) {

                    if (packet[0] == 'r') {

                        orb_set_color(RED);

                        usb_puts("received RED packet\r\n");

                        count++;

                    }

                    else if (packet[0] == 'b') {

                        orb_set_color(BLUE);

                        usb_puts("received BLUE packet\r\n");

                        count++;

                    }

                }

            }

        }

        delay_ms(1000);

        wl_basic_send_global_packet(1, "r", 2);

        usb_puts("sent RED packet\r\n");

        delay_ms(1000);

        wl_basic_send_global_packet(1, "b", 2);

        usb_puts("sent BLUE packet\r\n");

    }

    delay_ms(1000);

        wl_terminate();

    orb_set_color(GREEN);

    delay_ms(2000);

    return 0;

}