Colony

#include <dragonfly_lib.h>

#include <wireless.h>

#include <xbee.h>

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

#include "global.h"

#include "station_hardware.h"

#include "test_comm.h"

#include "test_bom.h"

#include "test_rangefinders.h"

#include "test_motors.h"

#include "test_encoders.h"

bool communication_ok=false;

bool require_comm (void)

{

        if (!communication_ok)

                communication_ok=test_comm ();

        if (!communication_ok)

                usb_puts ("Communications required. Stop." NL);

        return communication_ok;

}

void test_all (void)

{

        // test_all tests comm itself because we want to test comm, even if it

        // succeeded before (that's what test_all means).

        if (!test_comm ())

        {

                usb_puts ("Communications require. Stop." NL);

                return;

        }

        test_bom (true, true);

        test_rangefinders ();

        test_encoders ();

        test_motors ();

}

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 (true, true); break;

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

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

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

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

                        default: break; // ignore it

//                                usb_puts("Received invalid input ");

//                                usb_putc(choice);

//                                usb_puts(NL);

                }

        }

}

void server_main (void)

{

        // Set the orbs to green/yellow

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

        while (1);

}

int main (void)

{

#include <dragonfly_lib.h>

#include <wireless.h>

    dragonfly_init(0);

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

        xbee_init ();

        wl_init();

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

        if (button2_read ())

                server_main ();

        else

                interactive_main ();

        while (1);

}