Colony

 /**

 * @file rangefinder_with_noise.c

 * @author Colony Project

 *

 * @brief Returns rangefinder units with noise given distance in cm.

 *

 * Adds a random, normal amount of noise to a rangefinder

 * value given in cm. Returns approximate IR units .

 **/

double get_rangefinder(double dist_in_cm)

{

	double cm, IR, noise;

	// gauss_init(1, 0); // Params are chaos scalar and seed

	// noise = gauss_noise(void); // Gives ther noise

	cm = dist_in_cm; // Enter value between 7 and 18 for accurate results

	IR = 15.4005 * cm; // 15.4005 is an experimental value

	/*  Rangefinder is good for values between 7 and 18.

	*	3 < cm < 7 AND 18 < cm < 22 are significantly more erroneous.

	*	cm < 3 and cm > 22, rangefinder reports -1

	*/

	if ( cm < 3 || cm > 22 )

	{

		return -1;

	}

	else if( cm < 7 || cm > 18 )

	{

		gauss_init(2, 0);

		noise = gauss_noise();

		return IR + noise;

	}

	else 

	{

		gauss_init(1, 0);

		noise = gauss_noise();

		return IR + noise;

	}

}

#include <stdlib.h>

#include <math.h>

double Xr, Yr, Tr, Xc, Yc, Rc; // (X,Y,theta for ray) (X,Y,radius for circle)

boolean rayHits(double Xr,double Yr,double Tr,double Xc,double Yc,double Rc)

{

	double m, b, D, t;

	m = tan(Tr);

	b= -m * Xr + Yr;

	D = (abs(Yc - m * Xc - b)) / (sqrt(m * m + 1);

	if (D < Rc )

	{

		return true;

	}

	else

	{

		return false;

	}

	return false;

}

/*

*

*

*

*

*

*

*

*

*/

double a, b, c, t1, t2;

t1 = -(

((Xr - Xc)^2 + 2*cos(Tr)*(