Colony

#include <stdio.h>

#include <math.h>

#include "motion.h"

#define CUTOFF          120

#define ABS(x) (x<0?-x:x)

#define TIME      1 /*sec*/

#define ROBOT_WIDTH 131 /*mm*/

/** move will move a robot from its initial position, (x,y), and theta (in radians) to a new position given speed.

 * (x,y) and theta will be updated by the move function instead of returning a value

 * (x,y) is some kind of absolute position in the "world", let's make (0,0) the top left of the "world"

 * theta will an angle be between 0 - 2*Pi (0 being faces east)

 * speed is between 0 - 255, there is some magical cutoff point before the motors actually starts running

 * move will return 0 if successful

 **/

int move (int *x, int *y, double *theta, int speed1, int speed2) {

  if (*theta < 0 || *theta > 2*M_PI) return 1;

  if (speed1 < 0 || speed1 > 255) return 1;

  if (speed2 < 0 || speed2 > 255) return 1;

  /* if speed is lower than the cut off, don't move */

  if (ABS(speed1) < CUTOFF) {

    speed1 = 0;

  }

  if (ABS(speed2) < CUTOFF) {

    speed2 = 0;

  }

  double radius;

  if (speed1 == speed2) {

          /* go straight */

        *x += cos(*theta) * speed1;

        *y += sin(*theta) * speed1;

          return 0;

  }

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

  double t = speed1 / radius;

  double newx = radius * sin(t);

  double newy = radius - radius * cos(t);

  *x += newx * cos(*theta);

  *y += newx * sin(*theta);

  *x += newy * - sin(*theta);

  *y += newy * cos(*theta);

  *theta = fmod((t + *theta), (2 * M_PI));

  if (*theta<0) *theta += 2 * M_PI;

  return 0;

}