Colony

#include "dump_robot.h"

#include <dragonfly_lib.h>

#include "global.h"

#include "comm_robot.h"

void dump_robot_encoders (void)

{

        int16_t left, right;

        char c;

        robot_reset_encoders ();

        // TODO implement resetting the encoders (r key, for example)

        while (usb_getc_nb (&c)==-1)

        {

                robot_read_encoders (&left, &right);

                usb_puts ("Left:  "); usb_puti (left ); usb_puts (NL);

                usb_puts ("Right: "); usb_puti (right); usb_puts (NL);

                usb_puts (NL NL);

        }

}

void dump_robot_rangefinders (void)

{

        uint16_t rf[5];

        char c;

        while (usb_getc_nb (&c)==-1)

        {

                // First read all of the values, then print them, because it's much more readable that way.

                for (uint8_t i=0; i<5; ++i)

                {

                        robot_read_rangefinder (i, &rf[i]);

                }

                for (uint8_t i=0; i<5; ++i)

                {

                        usb_puti (i);

                        usb_puts (": ");

                        usb_puti (rf[i]);

                        usb_puts (NL);

                }

                usb_puts (NL NL);

        }

}

void dump_robot_bom (void)

{

        int16_t bom[16];

        char c;

        while (usb_getc_nb (&c)==-1)

        {

                // First read all of the values, then print them, because it's much more readable that way.

                //for (uint8_t i=0; i<16; ++i)

                        //robot_read_bom (i, &bom[i]);

                robot_read_bom_all (bom);

                for (uint8_t i=0; i<16; ++i)

                {

                        if (i<10) usb_puts (" ");

                        usb_puti (i);

                        usb_puts (": ");

                        usb_puti (bom[i]);

                        usb_puts (NL);

                }

                usb_puts (NL NL);

        }

}