Colony

#include <stdio.h>

#define        FORWARD          1

#define        BACKWARD  0

#define CUTOFF          120

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

 * (x,y) 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 - 359 (0 degree being faces east)

 * dir is either FORWARD or BACKWARD

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

 * 2 pairs of dir and speed, one for each motor

 * move will return 0 if successful

 */

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

  if (theta < 0 || theta > 359) return 1;

  if (dir1 != FORWARD && dir1 != BACKWARD) return 1;

  if (dir2 != FORWARD && dir2 != BACKWARD) 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 (speed1 < CUTOFF) {

    speed1 = 0;

  }

  if (speed2 < CUTOFF) {

    speed2 = 0;

  }

  /* FAKE VALUES */

  (*x) += speed1-CUTOFF;

  (*y) += speed1-CUTOFF;

  /* d = distance between the two wheels = 12 cm

   * v1 = speed of the first wheel

   * v2 = speed of the second wheel

   *

   * Unless v1 = v2, if left at that point, the robot will go in a circle at some radius.

   *

   * This radius is = dv1/(v1-v2).

   * Since we could measure the displacement of a wheel, we could figure out what the overall angular displacement is using r(theta) = arc length  and then use trig after that to figure out x, y coordinate displacement.

   */

  return 0;

}

int main () {

  int x = 0;

  int y = 0;

  printf("%d %d\n",x,y);

  move(&x,&y,0,FORWARD,150,FORWARD,150);

  printf("%d %d\n",x,y);

  return 0;

}