Colony

#include "test_rangefinders.h"

#include "global.h"

#include "hardware.h"

#include "comm_robot.h"

void test_rangefinder (uint8_t num)

{

        if (num>4) return;

        usb_puts("# Testing rangefinder ");

        usb_puti(num);

        usb_puts(NL);

        //        rotate to position

        //        

        for (uint8_t direction=1; direction<=2; ++direction)

        {

        //        for (wall positions up/down)

        //        {

        //                set wall position

        //                send (read)

        //                receive (data)

        //        }

                // Send data

                usb_puts ("data rangefinder ");

                usb_puti (num);

                usb_puts (" ");

                usb_puts (wall_direction_string (direction));

                usb_puts (" 1/1 2/2 3/3 4/4 5/5 6/6 7/7 8/8" NL);

        }

}

void test_rangefinder_all (void)

{

        usb_puts("# Testing rangefinders" NL);

        for (int n=0; n<5; ++n)

                test_rangefinder (n);

        usb_puts("# Testing rangefinders finished" NL);

}