Colony

#include "test_encoders.h"

#include "global.h"

#define num_measurements 5

#define velocity 200

#define delay 500

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 (delay);

                robot_set_motors (direction1, 0, direction2, 0);

                delay_ms (delay);        // FIXME wait for steady state!

        //        send (turn motor on for time)

        //        wait (done)

        //        wait_stopped

        //        send (read encoders)

        //        receive (data)

        //        read encoders

        }

}

void test_encoders (void)

{

        usb_puts("Testing encoders" NL);

        test_encoders_direction (FORWARD, BACKWARD);

        test_encoders_direction (BACKWARD, FORWARD);

        //send data

        usb_puts("Testing encoders finished" NL);

}