void move_forward_full()
{
  move_forward( SPEED_FULL );
}

void move_forward_half()
{
  move_forward( SPEED_1_2 );
}


void move_forward( int speed )
{
  set_left_motors( speed );
  set_right_motors( speed );
}

void move_backward_full()
{
  set_left_motors(-SPEED_FULL);
  set_right_motors(-SPEED_FULL);
  return;
}

void move_backward( int speed )
{
  set_left_motors(- speed );
  set_right_motors(- speed );
  return;
}

void turn_right( )
{
  turn_right_tick( RIGHT_TICKS );
}

void turn_left( )
{
  turn_left_tick( LEFT_TICKS );
}

void turn_right_tick( int ticks )
{
  int target;

  target = read_encoder(LEFT_ENCODER) + read_encoder(RIGHT_ENCODER) + ticks;


  
  while( read_encoder(LEFT_ENCODER) +
	 read_encoder(RIGHT_ENCODER) 
	 < target )
    {
      set_right_turn();
    }

  motor_stop();
  return;
}


void turn_left_tick( int ticks )
{
  int target;

  target = read_encoder(LEFT_ENCODER) + read_encoder(RIGHT_ENCODER) + ticks;
  
  while( read_encoder(LEFT_ENCODER) +
	 read_encoder(RIGHT_ENCODER) 
	 < target )
    {
      set_left_turn();
    }
  
  motor_stop();
  return;
}

void set_left_turn()
{
  set_left_motors(-SPEED_FULL);
  set_right_motors(SPEED_FULL);
}

void set_right_turn()
{
  set_left_motors(SPEED_FULL);
  set_right_motors(-SPEED_FULL);
}

void set_right_motors(int speed)
{
  motor(RIGHT_MOTOR_1, speed);
  motor(RIGHT_MOTOR_2, speed);
}

void set_left_motors(int speed)
{
  motor(LEFT_MOTOR_1, speed);
  motor(LEFT_MOTOR_2, speed);
}

void turn_180()
{
  turn_180_right();
}
void turn_180_right()
{
  turn_right_tick( BACKWARD_TICKS_RIGHT );
}
void turn_180_left()
{
  turn_left_tick( BACKWARD_TICKS_LEFT );
}

void run_roller()
{
  motor(ROLLER_MOTOR, -SPEED_FULL);
  return;
}

void stop_roller()
{
  off(ROLLER_MOTOR);
}

void motor_stop()
{
  off(LEFT_MOTOR_1);
  off(RIGHT_MOTOR_1);
  off(LEFT_MOTOR_2);
  off(RIGHT_MOTOR_2);
}

void left_motor_stop()
{
  off(LEFT_MOTOR_1);
  off(LEFT_MOTOR_2);
}

void right_motor_stop()
{
  off(RIGHT_MOTOR_1);
  off(RIGHT_MOTOR_2);
}
void servo_test()
{
  int i = 0;
  
  enable_servos();

  while(1){
    i = 15*knob()+30;
    servo(5,i);
    printf("%d\n",i);
    sleep(0.1);
  }

  disable_servos();
}

/*
  Shaft Encoders
  --------------
  This function works by moving the robot forward for an inputted
  number of ticks.  This can be changed later to a condition such
  as hitting a line.  When a change is detected upon finishing a 
  a while loop, the function will make the appropriate changes to 
  motor speed to compensate and put the robot on the right path.

  REQUIRES:  encoders MUST be enabled before this function is called!
*/

void fd_encoder(int ticks)
{
  /*  Assume that the encoders already have a reading on them.
      The inputted ticks is the absolute amount of ticks it should
      travel.  
   */
  int realRightTicks = read_encoder(RIGHT_ENCODER) + ticks;
  int realLeftTicks = read_encoder(LEFT_ENCODER) + ticks;
  int rightEncoder = 0;
  int leftEncoder = 0;
  int threshold  = 8;
  int leftDiff, rightDiff; /* keeps the amount of distance left to travel
			      on each side */
  /* Threshold defines the maximum number of ticks an encoder can be
     behind another to do a soft turn.  More than this will make the
     robot do a hard turn.
  */

  int dif = 0;

  rightEncoder = read_encoder(RIGHT_ENCODER);
  leftEncoder  = read_encoder(LEFT_ENCODER);

  while ( (rightEncoder < realRightTicks) &&
	  (leftEncoder < realLeftTicks) )
  {
    
    rightDiff = realRightTicks - rightEncoder ;
    leftDiff =  realLeftTicks - leftEncoder ;
    dif = abs( leftDiff - rightDiff );
    
    /* L << R */
    /* turn hard right */
    if ( leftDiff > rightDiff &&
	(dif >= threshold) )
    {
      printf("turning hard right. %d %d \n", rightDiff, leftDiff);
      turn_hard_right();
    }
    
    /* L < R  */
    /* turn soft right */
    else if ((leftDiff > rightDiff) &&
	     (dif < threshold) )
    {
      printf("turning soft right. %d %d \n", rightDiff, leftDiff);
      turn_soft_right();
    }

    /* L = R  */
    /* go forward */
    else if (leftDiff == rightDiff)
      {
	printf("going straight. %d %d \n", rightDiff, leftDiff);
	move_forward( SPEED_2_3 );
      }
    
    /* R < L  */
    /* turn soft left */
    else if ((rightDiff > leftDiff) &&
	     (dif < threshold) )
      {
	printf("turning soft left. %d %d \n", rightDiff, leftDiff);
	turn_soft_left();
      }
    
    /* R << L */
    /* turn hard left */
    else if ((rightDiff > leftDiff) &&
	     (dif >= threshold) )
      {
	printf("turning hard left. %d %d \n", rightDiff, leftDiff);
	turn_hard_left();
      }


    if ( flushFront() )
      {
	printf("stopping\n");
	motor_stop();
	break;
      }
    rightEncoder = read_encoder(RIGHT_ENCODER);
    leftEncoder  = read_encoder(LEFT_ENCODER);
  
  }
  
}

/*
void turn_hard_right()
{
  set_left_motors(SPEED_FULL);
  set_right_motors(SPEED_2_3);
}

void turn_soft_right()
{
  set_left_motors(SPEED_FULL);
  set_right_motors(SPEED_5_6);
}

void turn_soft_left()
{
  set_left_motors(SPEED_5_6);
  set_right_motors(SPEED_FULL);
}

void turn_hard_left()
{
  set_left_motors(SPEED_2_3);
  set_right_motors(SPEED_FULL);
}
*/


void turn_hard_right()
{
  set_left_motors(SPEED_2_3);
  set_right_motors(SPEED_1_3);
}

void turn_soft_right()
{
  set_left_motors(SPEED_2_3);
  set_right_motors(SPEED_1_2);
}

void turn_soft_left()
{
  set_left_motors(SPEED_1_2);
  set_right_motors(SPEED_2_3);
}

void turn_hard_left()
{
  set_left_motors(SPEED_1_3);
  set_right_motors(SPEED_2_3);
}


void step_forward()
{
  /*
  set_left_motors(SPEED_FULL);
  set_right_motors(SPEED_FULL);
  sleep( STEP_FORWARD_SLEEP_TIME );
  motor_stop();
  */

  fd_encoder(5);
}

void step_backward()
{
  move_backward( SPEED_2_3 );
  sleep( STEP_BACK_SLEEP_TIME );
  motor_stop();
  
}