Colony

#include <stdlib.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      ();

		data_rangefinder[i]=robot_read_rangefinder (num);

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

}

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

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