control1_motor.lst

Embedded camera control program.

.................... 		y = y + p[i] * t2; 
.................... 	} 
....................  
.................... 	if (quad == 2 || quad == 1) 
.................... 		y = -y;  // correct sign 
....................  
.................... 	return (y); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float sin(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the sine value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float sin(float x) 
.................... { 
.................... 	return cos(x - PI_DIV_BY_TWO); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float tan(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the tangent value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float tan(float x) 
.................... { 
....................    float c, s; 
....................  
....................    c = cos(x); 
.................... 	if (c == 0.0) 
.................... 	   return (1.0e+36); 
....................  
....................    s = sin(x); 
....................    return(s/c); 
.................... } 
....................  
....................  
....................  
.................... float const pas[3] = {0.49559947, -4.6145309, 5.6036290}; 
.................... float const qas[3] = {1.0000000,  -5.5484666, 5.6036290}; 
....................  
.................... float ASIN_COS(float x, int n) 
.................... { 
....................    float y, res, r, y2; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x <-1 || x > 1) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 0.5) 
....................    { 
....................       y = sqrt((1.0 - y)/2.0); 
....................       n += 2; 
....................    } 
....................  
....................    y2=y*y; 
....................  
....................    res = pas[0]*y2 + pas[1]; 
....................    res = res*y2 + pas[2]; 
....................  
....................    r = qas[0]*y2 + qas[1]; 
....................    r = r*y2 + qas[2]; 
....................  
....................    res = y*res/r; 
....................  
....................    if (n & 2)     // |x| > 0.5 
....................       res = PI_DIV_BY_TWO - 2.0*res; 
....................    if (s) 
....................       res = -res; 
....................    if (n & 1)           // take arccos 
....................       res = PI_DIV_BY_TWO - res; 
....................  
....................    return(res); 
.................... } 
....................  
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float asin(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arcsine value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float asin(float x) 
.................... { 
....................    float r; 
....................  
....................    r = ASIN_COS(x, 0); 
....................    return(r); 
.................... } 
....................  
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float acos(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arccosine value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float acos(float x) 
.................... { 
....................    float r; 
....................  
....................    r = ASIN_COS(x, 1); 
....................    return(r); 
.................... } 
....................  
.................... float const pat[4] = {0.17630401, 5.6710795, 22.376096, 19.818457}; 
.................... float const qat[4] = {1.0000000,  11.368190, 28.982246, 19.818457}; 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float atan(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the arctangent value of the value x. 
.................... // Date : N/A 
.................... // 
.................... float atan(float x) 
.................... { 
....................    float y, res, r; 
....................    int1 s, flag; 
....................  
....................    s = 0; 
....................    flag = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       y = -y; 
....................    } 
....................  
....................    if (y > 1.0) 
....................    { 
....................       y = 1.0/y; 
....................       flag = 1; 
....................    } 
....................  
....................    res = pat[0]*y*y + pat[1]; 
....................    res = res*y*y + pat[2]; 
....................    res = res*y*y + pat[3]; 
....................  
....................    r = qat[0]*y*y + qat[1]; 
....................    r = r*y*y + qat[2]; 
....................    r = r*y*y + qat[3]; 
....................  
....................    res = y*res/r; 
....................  
....................  
....................    if (flag)                              // for |x| > 1 
....................       res = PI_DIV_BY_TWO - res; 
....................    if (s) 
....................       res = -res; 
....................  
....................    return(res); 
.................... } 
....................  
.................... ///////////////////////////////////////////////////////////////////////////// 
.................... //	float atan2(float y, float x) 
.................... ///////////////////////////////////////////////////////////////////////////// 
.................... // Description :computes the principal value of arc tangent of y/x, using the 
.................... // signs of both the arguments to determine the quadrant of the return value 
.................... // Returns : returns the arc tangent of y/x. 
.................... // Date : N/A 
.................... // 
....................  
....................  
.................... float atan2(float y,float x) 
.................... { 
....................    float z; 
....................    int1 sign; 
....................    int quad; 
....................    sign=0; 
....................    quad=0; //quadrant 
....................    quad=((y<=0.0)?((x<=0.0)?3:4):((x<0.0)?2:1)); 
....................    if(y<0.0) 
....................    { 
....................       sign=1; 
....................       y=-y; 
....................    } 
....................    if(x<0.0) 
....................    { 
....................       x=-x; 
....................    } 
....................    if (x==0.0) 
....................    { 
....................       if(y==0.0) 
....................       { 
....................       #ifdef _ERRNO 
....................       { 
....................          errno=EDOM; 
....................       } 
....................       #endif 
....................       } 
....................       else 
....................       { 
....................          if(sign) 
....................          { 
....................          return (-(PI_DIV_BY_TWO)); 
....................          } 
....................          else 
....................          { 
....................          return (PI_DIV_BY_TWO); 
....................          } 
....................       } 
....................    } 
....................    else 
....................    { 
....................       z=y/x; 
....................       switch(quad) 
....................       { 
....................          case 1: 
....................          { 
....................             return atan(z); 
....................             break; 
....................          } 
....................          case 2: 
....................          { 
.................... //            return (atan(z)+PI_DIV_BY_TWO);  //2L3122 
....................             return (PI-atan(z)); 
....................             break; 
....................          } 
....................          case 3: 
....................          { 
....................             return (atan(z)-PI); 
....................             break; 
....................          } 
....................          case 4: 
....................          { 
....................             return (-atan(z)); 
....................             break; 
....................          } 
....................       } 
....................    } 
.................... } 
....................  
.................... //////////////////// Hyperbolic functions //////////////////// 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float cosh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic cosine value of x 
.................... // Returns : returns the hyperbolic cosine value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float cosh(float x) 
.................... { 
....................    return ((exp(x)+exp(-x))/2); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float sinh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic sine value of x 
.................... // Returns : returns the hyperbolic sine value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float sinh(float x) 
.................... { 
....................  
....................    return ((exp(x) - exp(-x))/2); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float tanh(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the hyperbolic tangent value of x 
.................... // Returns : returns the hyperbolic tangent value of x 
.................... // Date : N/A 
.................... // 
....................  
.................... float tanh(float x) 
.................... { 
....................    return(sinh(x)/cosh(x)); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float frexp(float x, signed int *exp) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : breaks a floating point number into a normalized fraction and an integral 
.................... // power of 2. It stores the integer in the signed int object pointed to by exp. 
.................... // Returns : returns the value x, such that x is a double with magnitude in the interval 
.................... // [1/2,1) or zero, and value equals x times 2 raised to the power *exp.If value is zero, 
.................... // both parts of the result are zero. 
.................... // Date : N/A 
.................... // 
....................  
....................  
....................  
.................... #define LOG2 .30102999566398119521 
.................... float frexp(float x, signed int *exp) 
.................... {