/* global flag denoting robot is frozen */
int stopped = 1;
float speed = 0.0;

void init()
{
  /* calibrateThresholds(); */
  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);
}

void checkStopped() {
  int left_diff, right_diff;

  /* stores state for robotStopped */
  int left_ticks = 0;
  int right_ticks = 0;
  int left_temp = 0;
  int right_temp = 0;

  enable_encoder(LEFT_ENCODER);
  enable_encoder(RIGHT_ENCODER);

  while( 1 ) {

    left_temp = read_encoder(LEFT_ENCODER);
    right_temp = read_encoder(RIGHT_ENCODER);
    
    left_diff = left_temp - left_ticks;
    right_diff = right_temp - right_ticks;
    
    left_ticks = left_temp;
    right_ticks = right_temp;
    
    if( left_diff == 0
	&& right_diff == 0 )
      {
	stopped = 1;
      }
    else
      {
	stopped = 0;
      }

    speed =( (float) (left_diff + right_diff) ) / 0.2;

    sleep( 0.5 );

  }
}