Colony

#include "test_motors.h"

#include "global.h"

#include "comm_robot.h"

// NB don't produce overflows!

#define vel_min 160

#define vel_max 250

#define vel_inc 10

#define vel_delay 200

static void test_motors_direction_velocity (uint8_t direction1, uint8_t direction2, uint8_t velocity)

{

        robot_set_motors (direction1, velocity, direction2, velocity);

        delay_ms (vel_delay);

        //        wait (steady)

        //        

        //        reset_encoders ();

        //        for (measurements)

        //        {

        //                measure left/right

        //                wait

        //        }

}

static void test_motors_direction (uint8_t direction1, uint8_t direction2)

{

        // Use 16 bit variable for vel to avoid problems with overflow (if vel=250 and 10 is added, it would be 260-256=4

        // which is still smaller than vel_max). There are more elegant solutions to this problem, but this one is

        // easy and performance is not an issue here anyway.

        for (uint16_t vel=vel_min; vel<=vel_max; vel+=vel_inc)

                test_motors_direction_velocity (direction1, direction2, vel);

        for (uint16_t vel=vel_max; vel>=vel_min; vel-=vel_inc)

                test_motors_direction_velocity (direction1, direction2, vel);

}

void test_motors (void)

{

        usb_puts("# Testing motors" NL);

        test_motors_direction (FORWARD, BACKWARD);

        test_motors_direction (BACKWARD, FORWARD);

        robot_set_motors_off ();

        //send data

        usb_puts("# Testing motors finished" NL);

}