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

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

        }

}

void test_encoder_all (void)

{

        usb_puts("# Testing encoders" NL);

        test_encoders_direction (FORWARD, BACKWARD);

        test_encoders_direction (BACKWARD, FORWARD);

        //send data

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

}