Colony

#include "test_motors.h"

#include "global.h"

#include "comm_robot.h"

#include "hardware_dynamos.h"

// ******************

// ** Data sending **

// ******************

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

        }

}

// ************************

// ** Internal functions **

// ************************

        // One way to do it

//        #define vel_inc 10

//        #define vel_steps 12

//        #define vel_min 140

//        #define vel_max 250

//        #define velocity_steady_delay 400

        // More steps at smaller intervals. We don't need so much delay here.

        #define vel_inc 2

        #define vel_steps 56

        #define vel_min 140

        #define vel_max 250

        #define velocity_steady_delay 50

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

                // Wait some fixed time for the velocity to reach steady state. This could be improved.

                delay_ms (velocity_steady_delay);

                data_pwm[i]=pwm;

                // Reset the dynamos, wait some time, then read the dynamos.

                // Nothing else should go between dynamos_reset and dynamos_read.

                // Note that we are currently using busy waiting which will be totally wrong if there are any interrupts.

                dynamos_reset ();

                delay_ms (500);

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

                // Velocity readings are dynamo ticks per second.

                data_velocity_l[i]*=2;

                data_velocity_r[i]*=2;

                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)

{

        // The following equation must be true: vel_min+(vel_steps-1)*vel_inc=vel_max

        // Note that we're setting vel_max twice, once going up and once going down. It's not important for vel_max, but

        // vel_min is also reached twice, once at the beginning of accelerating and once at the end of decelerating, and

        // this one is important.

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

}

// **********************

// ** Public functions **

// **********************

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

        }

}