Colony

#include "test_motors.h"

#include "global.h"

#include "comm_robot.h"

// NB don't produce overflows!

#define vel_min 100

#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

        //        }

}

#define acceleration_increasing 1

#define acceleration_decreasing 2

// TODO: pass number of steps instead

static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t vel_start, uint8_t vel_end, int8_t vel_step, char *acceleration_string)

{

        // 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.

        // Distinguish between start>=end/start<end because we have to use the corresponding stop condition.

        if (vel_end>=vel_start)

                for (uint16_t vel=vel_start; vel<=vel_end; vel+=vel_step)

                        test_motors_direction_velocity (direction1, direction2, vel);

        else

                for (uint16_t vel=vel_start; vel>=vel_end; vel+=vel_step)

                        test_motors_direction_velocity (direction1, direction2, vel);

        // Send data

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

        {

                uint8_t direction=(i==1)?direction1:direction2;

                if (direction!=motor_direction_off)

                {

                        usb_puts ("data motor ");

                        usb_puti (i);

                        usb_puts (" ");

                        usb_puts (motor_direction_string (direction));

                        usb_puts (" ");

                        usb_puts (acceleration_string);

                        usb_puts (" ");

                        usb_puts (" 160/100 170/200 180/300 190/400 200/500 210/600 220/700" NL);

                }

        }

}

static void test_motors_direction (uint8_t direction1, uint8_t direction2)

{

        test_motors_direction_acceleration (direction1, direction2, vel_min, vel_max, vel_inc, "increasing");

        test_motors_direction_acceleration (direction1, direction2, vel_max, vel_min, -vel_inc, "decreasing");

}

void test_motor_all (void)

{

        usb_puts("# Testing motors" NL);

        test_motors_direction (motor_direction_forward, motor_direction_backward);

        test_motors_direction (motor_direction_backward, motor_direction_forward);

        robot_set_motors_off ();

        usb_puts("# Testing motors finished" NL);

}

void test_motor (uint8_t num)

{

        if (num==1)

        {

                test_motors_direction (motor_direction_forward, motor_direction_off);

                test_motors_direction (motor_direction_backward, motor_direction_off);

        }

        else if (num==2)

        {

                test_motors_direction (motor_direction_off, motor_direction_forward);

                test_motors_direction (motor_direction_off, motor_direction_backward);

        }

}