Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

#include "../common/comm_station_robot.h"

#include "global.h"

#include "hardware.h"

#include "tests.h"

#include "self_test.h"

#include "comm_server.h"

#include "comm_robot.h"

void interactive_main (void)

{

        while (1)

        {

                // Set the orbs to green/green

                orbs_set (0,255,0, 0,255,0);

                usb_puts (NL);

                usb_puts ("# Diagnostic station interactive mode" NL);

                usb_puts ("# Test (s)elf, (a)ll, (c)omm, (b)om, (r)angefinders, (m)otors, (e)ncoders" NL);

                char choice = usb_getc ();

                switch (choice) {

                        case 'c': case 'C': test_comm ();                                   break;

                        case 'a': case 'A': test_all ();                                    break; // test_all will test comm itself

                        case 'b': case 'B': if (require_comm ()) test_bom_all (true, true); break;

                        case 'r': case 'R': if (require_comm ()) test_rangefinder_all ();   break;

                        case 'm': case 'M': if (require_comm ()) test_motor_all ();         break;

                        case 'e': case 'E': if (require_comm ()) test_encoder_all ();       break;

                        case 's': case 'S': self_test ();                                   break;

                        default: break; // ignore it

                }

        }

}

int main_martin (void)

{

        dragonfly_init (0);

        orb_init ();

        // Initialize before using USB

        comm_server_init ();

        orb1_set (0, 0, 255);

        comm_robot_init ();

        orb2_set (0, 0, 255);

        int16_t left=69, right=105;

        while (1)

        {

                robot_set_motors (motor_direction_backward, 250, motor_direction_forward, 250);

                delay_ms (200);

        }

        // Encoder test

        while (0)

        {

                bool result=robot_read_encoders (&left, &right);

                usb_puti (result);

                usb_puts (" ");

                usb_puti (left);

                usb_puts (" ");

                usb_puti (right);

                usb_puts (NL);

                delay_ms (200);

        }

        while (1);

        return 0;

}

int main_john (void)

{

        dragonfly_init(0);

        orb_init();

        usb_init();

        analog_init(ADC_START);

        orbs_set(255, 0,255,255,0,255);

        rbom_debug();

        return 0;

}

int main_evan (void)

{

    dragonfly_init(0);

    usb_init ();

        usb_puts(NL NL NL);

        usb_puts("# Diagnostic station version " version_string " starting" NL);

    orb_init_pwm ();

        hardware_init ();

        comm_server_init ();

        comm_robot_init ();

        orb1_set (255, 0, 0); usb_puts("# Initializing wireless" NL);

        xbee_init ();

        wl_init();

        orb2_set (255, 0, 0); usb_puts("# Done" NL);

        // If button 1 is pressed after initialization of the wireless (which takes about 1s), run all tests.

        if (button1_read ())

                        test_all ();

        // If button 2 is pressed, go to interactive mode (green/green). If not, go to server mode (green/yellow).

        if (button2_read ())

                interactive_main ();

        else

        {

                // Set the orbs to green/yellow

                orbs_set (0,255,0, 255,127,0);

                server_main ();

        }

        while (1);

        return 0;

}

int main_default (void)

{

    dragonfly_init(0);

        // Initialize before using USB

        comm_server_init ();

        usb_puts(NL NL NL);

        usb_puts("# Diagnostic station version " version_string " starting" NL);

    orb_init_pwm ();

        hardware_init ();

        orb1_set (255, 0, 0); usb_puts("# Initializing wireless" NL);

        comm_robot_init ();

        orb2_set (255, 0, 0); usb_puts("# Done" NL);

        // If button 1 is pressed after initialization of the wireless (which takes about 1s), run all tests.

        if (button1_read ())

                        test_all ();

        // If button 2 is pressed, go to interactive mode (green/green). If not, go to server mode (green/yellow).

        if (button2_read ())

                interactive_main ();

        else

        {

                // Set the orbs to green/yellow

                orbs_set (0,255,0, 255,127,0);

                server_main ();

        }

        while (1);

        return 0;

}