control_motor2.lst

Embedded camera control program.

....................    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)?:4):((x<0.0)?: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)  
.................... {  
....................    float res;  
....................    int1 sign = 0;  
....................    if(x == 0.0)  
....................    {  
....................       *exp=0;  
....................       return (0.0);  
....................    }  
....................    if(x < 0.0)  
....................    {  
....................      x=-x;  
....................      sign=1;  
....................    }  
....................    if (x > 1.0)  
....................    {  
....................       *exp=(ceil(log10(x)/LOG2));  
....................       res=x/(pow(2, *exp));  
....................       if (res == 1)  
....................       {  
....................          *exp=*exp+1;  
....................           res=.5;  
....................       }  
....................    }  
....................    else  
....................    {  
....................       if(x < 0.5)  
....................       {  
....................          *exp=-1;  
....................          res=x*2;  
....................       }  
....................       else  
....................       {  
....................          *exp=0;  
....................           res=x;  
....................       }  
....................    }  
....................    if(sign)  
....................    {  
....................       res=-res;  
....................    }  
....................    return res;  
.................... }  
....................   
.................... //////////////////////////////////////////////////////////////////////////////  
.................... //	float ldexp(float x, signed int *exp)  
.................... //////////////////////////////////////////////////////////////////////////////  
.................... // Description : multiplies a floating point number by an integral power of 2.  
.................... // Returns : returns the value of x times 2 raised to the power exp.  
.................... // Date : N/A  
.................... //  
....................   
.................... float ldexp(float value, signed int exp)  
.................... {  
....................    return (value * pow(2,exp));  
.................... }  
.................... #endif  
....................  
.................... #include <string.h> 
....................  ////////////////////////////////////////////////////////////////////////////  
.................... ////        (C) Copyright 1996,2003 Custom Computer Services            ////  
.................... //// This source code may only be used by licensed users of the CCS C   ////  
.................... //// compiler.  This source code may only be distributed to other       ////  
.................... //// licensed users of the CCS C compiler.  No other use, reproduction  ////  
.................... //// or distribution is permitted without written permission.           ////  
.................... //// Derivative programs created using this software in object code     ////  
.................... //// form are not restricted in any way.                                ////  
.................... ////////////////////////////////////////////////////////////////////////////  
....................   
.................... #ifndef _STRING  
.................... #define _STRING  
.................... #include <stddef.h> 
....................  ///////////////////////////////////////////////////////////////////////////  
.................... ////        (C) Copyright 1996,2003 Custom Computer Services           ////  
.................... //// This source code may only be used by licensed users of the CCS C  ////  
.................... //// compiler.  This source code may only be distributed to other      ////  
.................... //// licensed users of the CCS C compiler.  No other use, reproduction ////  
.................... //// or distribution is permitted without written permission.          ////  
.................... //// Derivative programs created using this software in object code    ////  
.................... //// form are not restricted in any way.                               ////  
.................... ///////////////////////////////////////////////////////////////////////////  
....................   
.................... #ifndef _STDDEF  
....................   
.................... #define _STDDEF  
....................   
.................... #if sizeof(int *)==1  
.................... #define ptrdiff_t int  
.................... #else  
.................... #define ptrdiff_t long  
.................... #endif  
....................   
.................... #define size_t int  
.................... #define wchar_t char  
.................... #define NULL 0  
....................   
.................... #define offsetof(s,f) (offsetofbit(s,f)/8)  
....................   
.................... #endif  
....................  
.................... #include <ctype.h> 
....................  ////////////////////////////////////////////////////////////////////////////  
.................... ////        (C) Copyright 1996,2003 Custom Computer Services            ////  
.................... //// This source code may only be used by licensed users of the CCS C   ////  
.................... //// compiler.  This source code may only be distributed to other       ////  
.................... //// licensed users of the CCS C compiler.  No other use, reproduction  ////  
.................... //// or distribution is permitted without written permission.           ////  
.................... //// Derivative programs created using this software in object code     ////  
.................... //// form are not restricted in any way.                                ////  
.................... ////////////////////////////////////////////////////////////////////////////  
....................   
.................... #ifndef _CTYPE  
.................... #define _CTYPE  
....................   
.................... #define islower(x)  isamong(x,"abcdefghijklmnopqrstuvwxyz")  
.................... #define isupper(x)  isamong(x,"ABCDEFGHIJKLMNOPQRSTUVWXYZ")  
.................... #define isalnum(x)  isamong(x,"0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz")  
.................... #define isalpha(x)  isamong(x,"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz")  
.................... #define isdigit(x)  isamong(x,"0123456789")  
.................... #define isspace(x)  (x==' ')  
.................... #define isxdigit(x) isamong(x,"0123456789ABCDEFabcdef")  
.................... #define iscntrl(x)  (x<' ')  
.................... #define isprint(x)  (x>=' ')  
.................... #define isgraph(x)  (x>' ')  
.................... #define ispunct(x)  ((x>' ')&&!isalnum(x))  
....................   
.................... #endif  
....................   
....................  
....................   
....................   
....................   
.................... //////////////////////////////////////////////  
.................... //// Uncomment the following define to    ////  
.................... //// allow some functions to use a        ////  
.................... //// quicker algorithm, but use more ROM  ////  
.................... ////                                      ////  
.................... //// #define FASTER_BUT_MORE_ROM          ////  
.................... //////////////////////////////////////////////  
....................   
....................   
....................   
.................... /*Copying functions*/  
.................... /* standard template:  
....................    void *memmove(void *s1, void *s2, size_t n).  
....................    Copies max of n characters safely (not following ending '\0')  
....................    from s2 in s1; if s2 has less than n characters, appends 0 */  
....................   
.................... char *memmove(void *s1,char *s2,size_t n)  
.................... {  
....................    char *sc1;  
....................    char *sc2;  
....................    sc1=s1;  
....................    sc2=s2;  
....................    if(sc2<sc1 && sc1 <sc2 +n)  
....................       for(sc1+=n,sc2+=n;0<n;--n)  
....................          *--sc1=*--sc2;  
....................    else  
....................       for(;0<n;--n)  
....................          *sc1++=*sc2++;  
....................   return s1;  
....................   }  
....................   
.................... /* compiler ignored the name 'strcpy()'; perhaps, it's reserved? 
....................    Standard template: char *strcpy(char *s1, const char *s2)  
....................    copies the string s2 including the null character to s1*/  
....................   
.................... char *strcopy(char *s1, char *s2)  
.................... {  
............