Colony

#include <dragonfly_lib.h>

#include "global.h"

#include "station_hardware.h"

#include "test_comm.h"

#include "test_bom.h"

#include "test_rangefinders.h"

#include "test_encoders.h"

#include "test_motors.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_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, (e)ncoders, (m)otors" NL);

                char choice = usb_getc ();

                switch (choice) {

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

                        case 'a': case 'A': if (require_comm ()) test_all ();           break;

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

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

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

                        case 'm': case 'M': if (require_comm ()) test_motors ();        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)

{

        dragonfly_init(0);

        usb_init ();

        orb_init ();

        usb_puts(NL NL NL);

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

        hardware_init ();

        if (button2_read ())

                server_main ();

        else

                interactive_main ();

        while (1);

}