Colony

#include "test_encoders.h"

#include "global.h"

#include "comm_robot.h"

#define num_measurements 4

#define velocity 200

#define on_delay 500

#define off_delay 800

static char *direction_string (uint8_t direction)

{

        if (direction==direction_forward)

                return "forward";

        else if (direction==BACKWARD)

                return "backward";

        else

                return "?";

}

static void test_encoders_direction (uint8_t direction1, uint8_t direction2)

{

        // reset_encoders ();

        for (uint8_t m=0; m<num_measurements; ++m)

        {

                robot_set_motors (direction1, velocity, direction2, velocity);

                delay_ms (on_delay);

                robot_set_motors_off ();

                delay_ms (off_delay);

                // TODO use the robot_set_motors_time function, but wait for completion

                // message from robot

                // TODO instead of waiting a fixed time, wait until the wheels

                // are not moving any more, using the station's encoders.

        //        send (turn motor on for time)

        //        wait (done)

        //        wait_stopped

        //        send (read encoders)

        //        receive (data)

        //        read encoders

        }

        // Send data

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

        {

                usb_puts ("data encoder ");

                usb_puti (i);

                usb_puts (" ");

                usb_puts (direction_string (i==1?direction1:direction2));

                usb_puts (" 100/100 200/200 300/300 400/400 500/500 600/600 700/700" NL);

        }

}

void test_encoder_all (void)

{

        usb_puts("# Testing encoders" NL);

        test_encoders_direction (direction_forward, BACKWARD);

        test_encoders_direction (BACKWARD, direction_forward);

        usb_puts("# Testing encoders finished" NL);

}

void test_encoder (uint8_t num)

{

        if (!(num==1 || num==2)) return;

        // TODO implement single encoder testing

        test_encoder_all ();

}