/*starting part with shaft encoders */
#include <gyro.c>
/*calibration 36inches is 1000 encoder readings*/
/*void main() {
	start_press();
	lsb_ms_per_deg*=scale_factor;
	calibrate_gyro();
	start_process(update_angle());
	start_process(display_angle());
	go_dist(0.0, 100, 1000, 5000L);
	sleep(2.0);
	go_dist(90.0, 100, 500, 5000L);

	}
*/
/*get facing direction */

/*function to drive a certain shaft encoder distance, with a timeout */


/*encoder port = 8 !!!!!*/

/*TEST THIS FUNCTION!!!!!*/
/**/
void go_to_angle_t(float angle, long ms_timeout) {
	
/* To convert degrees to units of theta, multiply by lsb_ms_per_deg.
   Demo robot turns at about 120 deg/s when speed = 100 */

  float angle_target;
  float error, tolerance;
  int speed;
  long start_time;

  tolerance = 1.0 * lsb_ms_per_deg;
  angle_target = angle * lsb_ms_per_deg;
  error = theta - angle_target;
  msleep(100L);
  
  start_time = mseconds();
  while ((mseconds()-start_time) < ms_timeout) {
  	if ( fabs(error) > tolerance ) {

  		spin(100 * -sign(error));
   	 	while(fabs(error) > (80.0 * lsb_ms_per_deg)){
      		error = theta - angle_target;
   	 	}

    		spin(90 * -sign(error));
    		while(fabs(error) > (60.0 * lsb_ms_per_deg)){
      		error = theta - angle_target;
    		}

    		spin(80 * -sign(error));
    		while(fabs(error) > (10.0 * lsb_ms_per_deg)){
      		error = theta - angle_target;
    		}

    		/* Slam on the brakes */
    		spin(100 * sign(error));
    		msleep(100L);

    		/* Fine adjust either direction as necessary */
    		spin(70 * -sign(error));
    		while(fabs(error) > tolerance){
      		error = (theta - angle_target);
      		spin(70 * -sign(error));
    		}

    		spin(0);  

  	} /* if ( fabs(error)... */
  
 } /*possibly check bump sensors*/

} /* go_to_angle() */



void go_dist(float angle, int speed, int shaft_encoder_count, long timeout_ms) {
	/* The robot turns to the specified angle, then goes straight for 
   the specified duration. Note that speed must be greater than tweak, 
   and less than 100 - tweak. */

  long start_time;
  int lbumpsensor, rbumpsensor; /*bum sensors on front*/
  int bumpthresh=125; /*threshold for bump sensors*/
  int tweak = 20;
  float angle_target;
  float tolerance;

  tolerance = lsb_ms_per_deg * 0.5; /* +/-0.5 degree accuracy */

  /* first turn the robot */
  go_to_angle(angle);

  /* then go forward under gyro control */
  start_time = mseconds();
  angle_target = angle * lsb_ms_per_deg; 

  /*reset the encoder*/	  
  enable_encoder(8);
  reset_encoder(8);
	
	
 
 /*while we haven't reached where we wanted to go, and we've not timed out, GO
  if we've got to where we want to go, STOP, or if we've timed out, STOP*/

  while ((read_encoder(8) < shaft_encoder_count) && ((mseconds() - start_time) < timeout_ms)){
    	if (theta < (angle_target - tolerance)) {
      	/* Turn CCW a little */
      	motor(left, speed - tweak);
      	motor(right, speed + tweak);
    	} 
	else if (theta > (angle_target + tolerance)){
      	/* Turn CW a little */
      	motor(left, speed + tweak);
      	motor(right, speed - tweak);
    	} 
	else {
      	motor(left, speed);
      	motor(right, speed);
    	}  

    /* Check front sensors here, if both are bumped, terminate the while loop */
	 /*ASSUME '<bumpthresh' is the depressed position*/
    	lbumpsensor=analog(18);
    	rbumpsensor=analog(20);
    	if (lbumpsensor<bumpthresh || rbumpsensor<bumpthresh) {
		break;
    	}
  } /* while */
  

  /* Throw it in reverse briefly to stop quickly */
  motor(left, -100);
  motor(right, -100);
  msleep(20L);  
  motor(left, 0);
  motor(right, 0);

 /*printf("Encoder: %d\n", read_encoder(8));*/

}