Colony

#define TIME 1 /*sec*/

#define ROBOT_WIDTH 131 /*mm*/

#define FUDGE 10 /* minimum rangefinder distance until collision */

 **/

int move_robot(Robot* r)

{

	Pose old_pose = r->pose;

	short speed1 = r->shared->motor1;

	}

	double radius;

	if (speed1 == speed2) {

	  /* go straight */

	  r->pose.x += cos(theta) * speed1 / MOTOR_CONVERSION_FACTOR;

	  r->pose.y += sin(theta) * speed1 / MOTOR_CONVERSION_FACTOR;

	  return 0;

	}

	radius = ROBOT_WIDTH * speed1 / (speed1 - speed2);

	r->pose.x += newy * - sin(theta);

	r->pose.y += newy * cos(theta);

	r->pose.theta = (t+r->pose.theta) - (2 * M_PI * divide);

	if (r->pose.theta<0) r->pose.theta += 2 * M_PI;

	update_rangefinders(r);

	for (divide = 0; divide < 5; divide++) {

	    /* Lets just call this a collision... */

	    if (r->shared->ranges.d[divide] < FUDGE) {

		/* Restore x,y, but allow rotation */

		r->pose.x = old_pose.x;