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;

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

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

        {

                robot_read_encoders (&left, &right);

                usb_puti (left);

                usb_puts (" ");

                usb_puti (right);

                usb_puts (NL);

        }

}

void dump_robot_rangefinders (void)

{

        uint16_t rf;

        char c;

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

        {

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

                {

                        robot_read_rangefinder (i, &rf);

                        if (i!=0) usb_puts (" ");

                        usb_puti (rf);

                }

                usb_puts (NL);

        }

}

void dump_robot_bom (void)

{

        uint16_t bom;

        char c;

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

        {

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

                {

                        robot_read_rangefinder (i, &bom);

                        if (i!=0) usb_puts (" ");

                        usb_puti (bom);

                }

                usb_puts (NL);

        }

}