Colony

#include "test_encoders.h"

#include "global.h"

#include "comm_robot.h"

#include "hardware.h"

// TODO as parameters

#define num_measurements 4

#define velocity 200

#define on_delay 500

#define off_delay 800

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

// ** Data sending **

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

static void send_encoder_data (uint8_t num, uint8_t direction, 

        int16_t *data_station, int16_t *data_robot)

{

        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)

{

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

        }

        send_encoder_data (0, direction1, data_station_l, data_robot_l);

        send_encoder_data (1, direction2, data_station_r, data_robot_r);

        free (data_robot_r);

        free (data_robot_l);

        free (data_station_r);

        free (data_station_l);

}

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

// ** Public functions **

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

void test_encoder_all (void)

{

        usb_puts("# Testing encoders" NL);

        test_encoders_direction (motor_direction_forward, motor_direction_backward);

        test_encoders_direction (motor_direction_backward, motor_direction_forward);

        usb_puts("# Testing encoders finished" NL);

}

void test_encoder (uint8_t num)

{

        if (num==1)

        {

                test_encoders_direction (motor_direction_forward, motor_direction_off);

                test_encoders_direction (motor_direction_backward, motor_direction_off);

        }

        else if (num==2)

        {

                test_encoders_direction (motor_direction_off, motor_direction_forward);

                test_encoders_direction (motor_direction_off, motor_direction_backward);

        }

}