/* move.c */

/* This file contains the functions needed to move the robot */

/* makes the robot move forward */
 void forward()
{
   /* 
   motor(0, -100);
   motor(2, 100);
   motor(4, -30);
   motor(5, -30); 
   */

  motor(0, -100);
  motor(2, 100);
  motor(4, -15);
  motor(5, -15);


}

/* go forward a set amount of distance */
void measured_forward(int dist)
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < dist)
    {
      forward();
    }
  ao();
}

/* Turn left for a specified amount of time */
void turn_left(float time)
{
  motor(0, 100);
  motor(2, -100);
  motor(4, -30);
  motor(5, -30);
  sleep(time);
  ao();
}

void untimed_turn_left()
{
  motor(0, 100);
  motor(2, -100);
  motor(4, -30);
  motor(5, -30);
  
}

/* Turn Right for a specified amount of time */
void turn_right(float time)
{
  motor(0, -100);
  motor(2, 100);
  motor(4, 30);
  motor(5, 30);
  sleep(time);
  ao();
}


void untimed_turn_right()
{
  motor(0, -100);
  motor(2, 100);
  motor(4, 30);
  motor(5, 30);
}

/* slow the robot down, and then back up a little */
void slow_then_backup(float time)
{
  motor(0, -30);
  motor(2, 30);
  motor(4, -20);
  motor(5, -20);
  sleep(.10);

  ao();
  sleep(.10);

  motor(0, 100);
  motor(2, -100);
  motor(4, 30);
  motor(5, 30);
  sleep(time);
}

void backup(float time)
{
  motor(0, 100);
  motor(2, -100);
  motor(4, 30);
  motor(5, 30);
  sleep(time);
  ao();

}
/* Put the robot in reverse */
void measured_reverse(int dist)
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < dist)
    {
      motor(0, 100);
      motor(2, -100);
      motor(4, 30);
      motor(5, 30);
    }
  ao();
}

void reverse()
{
  motor(0, 100);
  motor(2, -100);
  motor(4, 30);
  motor(5, 30);
}

/* Turn right ninety */
void right_90()
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < RIGHT_NINETY)
    {
      untimed_turn_right();
    }

  ao();
}

/* turn right one eighty */
void right_180()
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < RIGHT_ONEEIGHTY)
    {
      untimed_turn_right();
    }

  ao();
}

/* turn left ninety degress */
void left_90()
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < LEFT_NINETY)
    {
      untimed_turn_left();
    }

  ao();
}

/* turn left one eighty */
void left_180()
{
  reset_encoder(ENCODER);
  while(read_encoder(ENCODER) < LEFT_ONEEIGHTY)
    {
      untimed_turn_left();
    }

  ao();
}


/* Makes the robot back into a wall to align itself */
void wall_align()
{
  int count;
  count = 0;

  while(!(digital(BACK_LEFT_BUMPER) && digital(BACK_RIGHT_BUMPER)))
    {
      if (count == 10000)
	{
	  measured_forward(5);
	  count = 0;
	}
      reverse();
      count++;
    }

  ao();
}

/*
 * SERVO STUFF
 */

/* Close the robot claw */
void close_claw()
{
  servo(0, 3300);
  servo(5, 600);
}

void claw_start_position()
{
  servo(0, 3700);
  servo(5, 100);
}

/* Open the robot claw, so that it is straight forward */
void open_claw_straight()
{
  servo(0, 1400);
  servo(5, 2600);
}

/* Open the left side of the claw */
void open_left()
{
  servo(5, 2600);
}

/* Close the left side of the claw */
void close_left()
{
  servo(5, 600);
}

/* Open the right side of the claw */
void open_right()
{
  servo(0, 1400);
}

/* Close the right side of the claw */
void close_right()
{
  servo(0, 3300);
}