Colony

#include <stdlib.h>

#include "test_rangefinders.h"

#include "global.h"

#include "hardware.h"

#include "comm_robot.h"

#define wall_min 0

#define wall_max 1000

#define wall_inc 100

static uint16_t rangefinder_position (uint8_t num)

{

        switch (num)

        {

                case 0:  return  100; break;

                case 1:  return  200; break;

                case 2:  return  300; break;

                case 3:  return  400; break;

                case 4:  return  500; break;

        }

        return 0;

}

static void test_rangefinder_direction (uint8_t num, uint16_t wall_start, uint16_t wall_step, uint8_t num_steps, char *wall_direction_string)

{

        // Allocate space for the data on the stack

        uint16_t *data_wall       =malloc (num_steps*sizeof (uint16_t));

        uint16_t *data_rangefinder=malloc (num_steps*sizeof (uint16_t));

        // Rotate the robot to the correct position

        turntable_rotate_to_position (rangefinder_position (num));

        uint16_t wall=wall_start;

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

        {

                wall_set_position (wall);

                data_wall[i]=wall_get_position      ();

                robot_read_rangefinder (num, &data_rangefinder[i]);

                wall+=wall_step;

        }

        // Send data

        usb_puts ("data rangefinder ");

        usb_puti (num);

        usb_puts (" ");

        usb_puts (wall_direction_string);

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

        {

                usb_putc (' ');

                usb_puti (data_wall[i]);

                usb_putc ('/');

                usb_puti (data_rangefinder[i]);

        }

        free (data_rangefinder);

        free (data_wall);

        usb_puts (NL);

}

void test_rangefinder (uint8_t num)

{

        if (num>4) return;

        usb_puts("# Testing rangefinder ");

        usb_puti(num);

        usb_puts(NL);

        test_rangefinder_direction (num, wall_min, wall_inc, 10, "out");

        test_rangefinder_direction (num, wall_max, -wall_inc, 10, "in");

}

void test_rangefinder_all (void)

{

        usb_puts("# Testing rangefinders" NL);

        for (int n=0; n<5; ++n)

                test_rangefinder (n);

        turntable_rotate_to_position(TURNTABLE_HOME_POSITION);        

        usb_puts("# Testing rangefinders finished" NL);

}