control1_motor.lst

Embedded camera control program.

.................... float ceil(float x) 
.................... { 
....................    return CEIL_FLOOR(x, 1); 
.................... } 
....................  
....................  //////////////////////////////////////////////////////////////////////////// 
.................... //	float fabs(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the absolute value of floating point number x 
.................... // Returns : returns the absolute value of x 
.................... // Date : N/A 
.................... // 
.................... #define fabs abs 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float fmod(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : Computes the floating point remainder of x/y 
.................... // Returns : returns the value of x= i*y, for some integer i such that, if y 
.................... // is non zero, the result has the same isgn of x na dmagnitude less than the 
.................... // magnitude of y. If y is zero then a domain error occurs. 
.................... // Date : N/A 
.................... // 
....................  
.................... float fmod(float x,float y) 
.................... { 
....................    float i; 
....................    if (y!=0.0) 
....................    { 
....................       i=(x/y < 0.0)? ceil(x/y): floor(x/y); 
....................       return(x-(i*y)); 
....................    } 
....................    else 
....................    { 
....................    #ifdef _ERRNO 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................    } 
.................... } 
....................  
.................... //////////////////// Exponential and logarithmic functions //////////////////// 
....................  
.................... #define LN2 0.6931471806 
....................  
.................... float const pe[6] = {0.000207455774, 0.00127100575, 0.00965065093, 
....................                      0.0554965651,  0.240227138,  0.693147172}; 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float exp(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (e^x) 
.................... // Date : N/A 
.................... // 
.................... float exp(float x) 
.................... { 
....................    float y, res, r; 
....................    signed int n; 
....................    int1 s; 
....................    #ifdef _ERRNO 
....................    if(x > 88.722838) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    n = (signed long)(x/LN2); 
....................    s = 0; 
....................    y = x; 
....................  
....................    if (x < 0) 
....................    { 
....................       s = 1; 
....................       n = -n; 
....................       y = -y; 
....................    } 
....................  
....................    res = 0.0; 
....................    *(&res) = n + 0x7F; 
....................  
....................    y = y/LN2 - (float)n; 
....................  
....................    r = pe[0]*y + pe[1]; 
....................    r = r*y + pe[2]; 
....................    r = r*y + pe[3]; 
....................    r = r*y + pe[4]; 
....................    r = r*y + pe[5]; 
....................  
....................    res = res*(1.0 + y*r); 
....................  
....................    if (s) 
....................       res = 1.0/res; 
....................    return(res); 
.................... } 
....................  
.................... /************************************************************/ 
....................  
.................... float const pl[4] = {0.45145214, -9.0558803, 26.940971, -19.860189}; 
.................... float const ql[4] = {1.0000000,  -8.1354259, 16.780517, -9.9300943}; 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float log(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the the natural log of x 
.................... // Date : N/A 
.................... // 
.................... float log(float x) 
.................... { 
....................    float y, res, r, y2; 
....................    signed n; 
....................    #ifdef _ERRNO 
....................    if(x <0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    if(x ==0) 
....................    { 
....................       errno=ERANGE; 
....................       return(0); 
....................    } 
....................    #endif 
....................    y = x; 
....................  
....................    if (y != 1.0) 
....................    { 
....................       *(&y) = 0x7E; 
....................  
....................       y = (y - 1.0)/(y + 1.0); 
....................  
....................       y2=y*y; 
....................  
....................       res = pl[0]*y2 + pl[1]; 
....................       res = res*y2 + pl[2]; 
....................       res = res*y2 + pl[3]; 
....................  
....................       r = ql[0]*y2 + ql[1]; 
....................       r = r*y2 + ql[2]; 
....................       r = r*y2 + ql[3]; 
....................  
....................       res = y*res/r; 
....................  
....................       n = *(&x) - 0x7E; 
....................  
....................       if (n<0) 
....................          r = -(float)-n; 
....................       else 
....................          r = (float)n; 
....................  
....................       res += r*LN2; 
....................    } 
....................  
....................    else 
....................       res = 0.0; 
....................  
....................    return(res); 
.................... } 
....................  
.................... #define LN10 2.30258509 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float log10(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the the log base 10 of x 
.................... // Date : N/A 
.................... // 
.................... float log10(float x) 
.................... { 
....................    float r; 
....................  
....................    r = log(x); 
....................    r = r/LN10; 
....................    return(r); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float modf(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description :breaks the argument value int integral and fractional parts, 
.................... // ach of which have the same sign as the argument.  It stores the integral part 
.................... // as a float in the object pointed to by the iptr 
.................... // Returns : returns the signed fractional part of value. 
.................... // Date : N/A 
.................... // 
....................  
.................... float modf(float value,float *iptr) 
.................... { 
....................    *iptr=(value < 0.0)? ceil(value): floor(value); 
....................    return(value - *iptr); 
.................... } 
....................  
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float pwr(float x,float y) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (x^y) 
.................... // Date : N/A 
.................... // 
.................... float pwr(float x,float y) 
.................... { 
....................    if(x>=0) 
....................      return(  exp(y*log(x)) ); 
....................    else 
....................      return(  -exp(y*log(-x)) ); 
.................... } 
....................  
....................  
.................... //////////////////// Power functions //////////////////// 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float pow(float x,float y) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the value (x^y) 
.................... // Date : N/A 
.................... // 
.................... float pow(float x,float y) 
.................... { 
....................    if(x>=0) 
....................      return(  exp(y*log(x)) ); 
....................    else 
....................      return(  -exp(y*log(-x)) ); 
.................... } 
....................  
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float sqrt(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the square root of x 
.................... // Date : N/A 
.................... // 
.................... float sqrt(float x) 
.................... { 
....................    float y, res; 
....................    BYTE *p; 
....................  
....................    #ifdef _ERRNO 
....................    if(x < 0) 
....................    { 
....................       errno=EDOM; 
....................    } 
....................    #endif 
....................  
....................    if( x<=0.0) 
....................       return(0.0); 
....................  
....................    y=x; 
....................    p=&y; 
....................    (*p)=(BYTE)((((int16)(*p)) + 127) >> 1); 
....................  
....................    do { 
....................       res=y; 
....................       y+=(x/y); 
....................       (*p)--; 
....................    } while(res != y); 
....................  
....................    return(res); 
.................... } 
....................  
....................  
....................  
.................... ////////////////////////////// Trig Functions ////////////////////////////// 
.................... #ifdef PI_DIV_BY_TWO 
.................... #undef PI_DIV_BY_TWO 
.................... #endif 
.................... #define PI_DIV_BY_TWO	1.570796326794896 
.................... #ifdef TWOBYPI 
.................... #undef TWOBYPI 
.................... #define TWOBYPI 			0.6366197724 
.................... #endif 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... //	float cos(float x) 
.................... //////////////////////////////////////////////////////////////////////////// 
.................... // Description : returns the cosine value of the angle x, which is in radian 
.................... // Date : 9/20/2001 
.................... // 
.................... float cos(float x) 
.................... { 
.................... 	float y, t, t2 = 1.0; 
.................... 	int quad, i; 
.................... 	float frac; 
.................... 	float p[4] = { 
.................... 		-0.499999993585, 
.................... 		 0.041666636258, 
.................... 		-0.0013888361399, 
.................... 		 0.00002476016134 
.................... 	}; 
....................  
.................... 	if (x < 0) x = -x;                  // absolute value of input 
....................  
.................... 	quad = (int)(x / PI_DIV_BY_TWO);    // quadrant 
.................... 	frac = (x / PI_DIV_BY_TWO) - quad;  // fractional part of input 
.................... 	quad = quad % 4;                    // quadrant (0 to 3) 
....................  
.................... 	if (quad == 0 || quad == 2) 
.................... 		t = frac * PI_DIV_BY_TWO; 
.................... 	else if (quad == 1) 
.................... 		t = (1-frac) * PI_DIV_BY_TWO; 
.................... 	else // should be 3 
.................... 		t = (frac-1) * PI_DIV_BY_TWO; 
....................  
.................... 	y = 0.999999999781; 
.................... 	t = t * t; 
.................... 	for (i = 0; i <= 3; i++) 
.................... 	{ 
.................... 		t2 = t2 * t;