Colony

#include "test_motors.h"

#include "global.h"

#include "comm_robot.h"

#include "hardware_encoders.h"

static void send_motor_data (uint8_t num, uint8_t direction, char *acceleration_string,

        uint8_t num_steps, uint8_t *data_pwm, int16_t *data_velocity)

{

        if (direction!=motor_direction_off)

        {

                usb_puts ("data motor ");

                usb_puti (num);

                usb_puts (" ");

                usb_puts (motor_direction_string (direction));

                usb_puts (" ");

                usb_puts (acceleration_string);

                usb_puts (" ");

                for (uint8_t i=0; i<num_steps; ++i)

                {

                        usb_putc (' ');

                        usb_puti (data_pwm[i]);

                        usb_putc ('/');

                        usb_puti (data_velocity[i]);

                }

                usb_puts (NL);

        }

}

static void test_motors_direction_acceleration (uint8_t direction1, uint8_t direction2, uint8_t pwm_start, uint8_t num_steps, int8_t pwm_step, char *acceleration_string)

{

        // Allocate space for the data on the stack

        uint8_t  *data_pwm       =malloc (num_steps*sizeof (uint8_t ));

        int16_t *data_velocity_l =malloc (num_steps*sizeof (uint16_t));

        int16_t *data_velocity_r =malloc (num_steps*sizeof (uint16_t));

        uint8_t pwm=pwm_start;

        for (uint8_t i=0; i<num_steps; ++i)

        {

                robot_set_motors (direction1, pwm, direction2, pwm);

// TODO better

#define vel_delay 400

                delay_ms (vel_delay); // TODO wait steady

                data_pwm[i]=pwm;

                encoders_reset ();

                // TODO delay

                encoders_read (&(data_velocity_l[i]), &(data_velocity_r[i]));

                pwm+=pwm_step;

        }

        send_motor_data (0, direction1, acceleration_string, num_steps, data_pwm, data_velocity_l);

        send_motor_data (1, direction2, acceleration_string, num_steps, data_pwm, data_velocity_r);

        robot_set_motors (motor_direction_off, 0, motor_direction_off, 0);

        free (data_velocity_r);

        free (data_velocity_l);

        free (data_pwm);

}

static void test_motors_direction (uint8_t direction1, uint8_t direction2)

{

        #define vel_inc 10

        #define vel_steps 10

        #define vel_min 140

        #define vel_max 250

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

        test_motors_direction_acceleration (direction1, direction2, vel_max, vel_steps, -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);

        }

}