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

        // Seems like we've got a stack overflow...see test_motors.c

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

        int16_t station_l=11, station_r=22, robot_l=33, robot_r=44;

        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 (&station_l, &station_r);

//                data_station_l[m]=station_l;

//                data_station_r[m]=station_r;

                // Read the robot encoders

                robot_read_encoders (&robot_l, &robot_r);

//                data_robot_l[m]=robot_l;

//                data_robot_r[m]=robot_r;

                //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, &station_l, &robot_l, 1);

        send_encoder_data (1, direction2, &station_r, &robot_r, 1);

        }

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

        }

}