/********************************************
*           file: NEWranging.c                                     *
********************************************/

float deg_to_rad;
int angle_error;
float distance_error;
int side; /* 1 for left, -1 for right */

void error_check(int which_way)
  {side=which_way; /* 1 for left, -1 for right */
   angle_error=theta_deviation();
   distance_error=wall_distance();}
   

int theta_deviation()  /* Note: + is away from the wall */
   {int deviation;
     b=c=d=0.;
     deviation = wall_angle_B() - 90;
     return deviation;}

int B;  /* Wall angle B */

int wall_angle_B()
 {return B=140 - wall_angle_C();} 

float wall_distance()
  {float B_rad = (float)(B)/deg_to_rad; /* convert angle B to radians*/
   float wall_offset;  /* distance to measured wall */
   wall_offset = c * sin(B_rad);
   return wall_offset;}

  int C;
  float a;
  float b ;
  float c;
  float d; /* used to determine if C > 90_ */
  float  A; /* A = included angle between readings */

int wall_angle_C()
 {int q;
  int temp_range;
  pan_deg(50*side); /*look to the side 50_ */
   while (!(int)(b))
     {b=range_avg();}
 
  pan_deg(60*side); /*look to the side 60_, used to find C > 90_ */
     while (!(int)(d))
     {d=range_avg();}
  pan_deg(90*side);
      while (!(int)(c))
     {c=range_avg();}  
  pan_deg(0);
 
  a=sqrt((b^2.) + (c^2.) - (2.*b*c*cos(A)));
  C=(int)(arcsin(c*sin(A)/a));
  if (d > b)  /* then C is more than 90_ */
    {C=180-C;}
  return C;}

void pan_deg(int deg)
  {float pan_temp;
   pan_temp=3035. + ((float)(deg)*18.30); 
   pan=(int)(pan_temp);
   servo3_on();
   pan=(int)(pan_temp);
   sleep(.8);
   pan=(int)(pan_temp);
   servo3_off();
   }

/********************END**********************/