Colony

#include "test_encoders.h"

#include "global.h"

#include "comm_robot.h"

#include "hardware.h"

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

// ** Data sending **

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

static void send_encoder_data (uint8_t num, uint8_t direction,

        int16_t *data_station, int16_t *data_robot, uint8_t num_measurements)

{

        if (direction!=motor_direction_off)

        {

                usb_puts ("data encoder ");

                usb_puti (num);

                usb_puts (" ");

                usb_puts (motor_direction_string (direction));

                usb_puts (" ");

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

                {

                        usb_putc (' ');

                        usb_puti (data_station[i]);

                        usb_putc ('/');

                        usb_puti (data_robot[i]);

                }

                usb_puts (NL);

        }

}

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

// ** Internal functions **

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

static void test_encoders_direction (uint8_t direction1, uint8_t direction2, uint8_t num_measurements, uint8_t velocity, uint16_t on_delay, uint16_t off_delay)

{

        // Allocate space for the data on the stack

        int16_t *data_station_l  =malloc (num_measurements*sizeof (uint16_t));

        int16_t *data_station_r  =malloc (num_measurements*sizeof (uint16_t));

        int16_t *data_robot_l    =malloc (num_measurements*sizeof (uint16_t));

        int16_t *data_robot_r    =malloc (num_measurements*sizeof (uint16_t));

        usb_puts ("# Resetting encoders" NL);

        dynamos_reset ();

        robot_reset_encoders ();

        for (uint8_t m=0; m<num_measurements; ++m)

        {

                robot_set_motors (direction1, velocity, direction2, velocity);

                delay_ms (on_delay);

                robot_set_motors_off ();

                delay_ms (off_delay);

                // Read the station dynamos

                dynamos_read (&(data_station_l[m]), &(data_station_r[m]));

                // Read the robot encoders

                robot_read_encoders (&(data_robot_l[m]), &(data_robot_r[m]));

                //usb_puts ("# I readed teh encoders: ");

                //usb_puti (data_robot_l[m]);

                //usb_puts (" ");

                //usb_puti (data_robot_r[m]);

                //usb_puts (NL);

        }

        send_encoder_data (0, direction1, data_station_l, data_robot_l, num_measurements);

        send_encoder_data (1, direction2, data_station_r, data_robot_r, num_measurements);

        free (data_robot_r);

        free (data_robot_l);

        free (data_station_r);

        free (data_station_l);

}

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

// ** Public functions **

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

// Encoder readings seem to be unreliable with these values (on too long?)

//#define NUM 4

//#define VEL 200

//#define ON_DELAY 500

//#define OFF_DELAY 800

// Testing values

#define NUM 8

#define VEL 200

#define ON_DELAY 150

#define OFF_DELAY 400

void test_encoder_all (void)

{

        usb_puts("# Testing encoders" NL);

        test_encoders_direction (motor_direction_forward , motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY);

        test_encoders_direction (motor_direction_backward, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY);

        usb_puts("# Testing encoders finished" NL);

}

void test_encoder (uint8_t num)

{

        if (num==1)

        {

                test_encoders_direction (motor_direction_forward , motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY);

                test_encoders_direction (motor_direction_backward, motor_direction_off, NUM, VEL, ON_DELAY, OFF_DELAY);

        }

        else if (num==2)

        {

                test_encoders_direction (motor_direction_off, motor_direction_forward , NUM, VEL, ON_DELAY, OFF_DELAY);

                test_encoders_direction (motor_direction_off, motor_direction_backward, NUM, VEL, ON_DELAY, OFF_DELAY);

        }

}