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

        //        }

}

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

        // 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 (" increasing ");

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

                }

        }

        // TODO code duplication

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

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

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

                }

        }

}

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

        }

}