rd_usedlocate_lib._c

全场地位系统：小车静止或移动过程中码盘进行全场定位,用的是avr单片机

/***********************************************************
*   函数库说明：差分定位数值计算函数库                     *
*   版本：      v1.00beta                                  *
*   作者：      王卓然                                     *
*   创建日期：  2008年3月29日                              *
* -------------------------------------------------------- *
*  [支 持 库]                                              *
*   支持库名称：                                           *
*   需要版本：                                             *
*   支持库说明：                                           *
* -------------------------------------------------------- *
*  [版本更新]                                              *
*   修改：                                                 *
*   修改日期：                                             *
*   版本：                                                 *
* -------------------------------------------------------- *
*  [版本历史]                                              *
* -------------------------------------------------------- *
*  [使用说明]                                              *
***********************************************************/

/********************
* 头 文 件 配 置 区 *
********************/
# include "RD_MacroAndConst.h"
# include "DL_Config.h"
# include "RD_UseDLocate_LIB.h"
# include "PF_Config.h"
# include "Math.h"

/********************
*   系 统 宏 定 义  *
********************/

/*------------------*
*   常 数 宏 定 义  *
*------------------*/

//轮子直径
#ifndef WHEEL_D_L
    #error No define WHEEL_D_L!
#endif
#ifndef WHEEL_D_R
    #error No define WHEEL_D_R!
#endif

//轮间距
#ifndef D_BTW_WHEEL
    #error No define D_BTW_WHEEL
#endif

//每圈线精度
#ifndef N_L
    #error No define N_L
#endif
#ifndef N_R
    #error No define N_R
#endif

//码盘倍频数
#ifndef MULTIPLE
    #error No define MULTIPLE
#endif

//计数器 到 弧长
# define K_L    ((PI * (WHEEL_D_L)) / ((MULTIPLE) * (N_L)))
# define K_R    ((PI * (WHEEL_D_R)) / ((MULTIPLE) * (N_R)))




/*------------------*
*   动 作 宏 定 义  *
*------------------*/

/********************
*  自定义类型声明区 *
********************/

/********************
*  模块结构体定义区 *
********************/

/********************
*   函 数 声 明 区  *
********************/
float Get_Absolute_Angle(void);
float fMod(float fNumberA,float fNumberB);
BOOL PROC_Difference_Locate(void);

/********************
*   模块函数声明区  *
********************/

/********************
*   模块变量声明区  *
********************/

/********************
*   全局变量声明区  *
********************/
float g_fLastAngle = 0.0;
float g_fX = 0.0;
float g_fY = 0.0;

/********************
*  全局变量声引用区 *
********************/
extern BOOL g_bRunFlagB;

/***********************************************************
*   函数说明：获取当前绝对角度函数                         *
*   输入：    无                                           *
*   输出：    角度值                                       *
*   调用函数：无                                           *
***********************************************************/
float Get_Absolute_Angle(void)
{
    return 
        (
            (float)g_lCounterR * (float)K_R 
             - (float)g_lCounterL * (float)K_L
         ) * (1.0 / (float)D_BTW_WHEEL);
}

/***********************************************************
*   函数说明：获取当前相对角度函数                         *
*   输入：    左编码器微分量，右编码器微分量               *
*   输出：    角度值（弧度）                               *
*   调用函数：无                                           *
***********************************************************/
float Get_Relative_Angle(INT32 lDCL,INT32 lDCR)
{
    if (lDCL == lDCR)
    {
        return 0.0;
    }
    
    return 
        (
            (float)lDCR * (float)K_R 
             - (float)lDCL * (float)K_L
         ) * (1.0 / (float)D_BTW_WHEEL);
}



/***********************************************************
*   函数说明：差分定位计算函数                             *
*   输入：    无                                           *
*   输出：    FALSE                                        *
*   调用函数：无                                           *
***********************************************************/
BOOL PROC_Difference_Locate(void)
{
    static INT32 s_lLastCounterL = 0;
    static INT32 s_lLastCounterR = 0;
    
    if ((g_lCounterLImage - s_lLastCounterL)
         == (g_lCounterRImage - s_lLastCounterR))
    {
        INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
        float fR = ((float)nDeltaCounterL * (float)K_L);
        
        g_fX += fR * cos(g_fLastAngle);
        g_fY += fR * sin(g_fLastAngle);
    }
    else
    {
        float fDeltaAngle,fAbsoluteAngle;
        //计算角度微元
        {
            float fTempAngle = Get_Relative_Angle
                            (
                                g_lCounterLImage,
                                g_lCounterRImage
                            );
            fDeltaAngle = (fTempAngle - g_fLastAngle);
            fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
    
            g_fLastAngle = fTempAngle;
        }
        //计算位置微元
        {
            INT16 nDeltaCounterL = (INT16)((INT32)g_lCounterLImage - (INT32)s_lLastCounterL);
            float fR = (((float)nDeltaCounterL * (float)K_L) * (1.0 / fDeltaAngle) 
                + (float)D_BTW_WHEEL * 0.5);

            fR = 2.0 * fR * sin(fDeltaAngle * 0.5);             //fR->l
        
            g_fX += fR * cos(fAbsoluteAngle);
            g_fY += fR * sin(fAbsoluteAngle);
        }
    }
    
    //数据更新
    s_lLastCounterL = g_lCounterLImage;
    s_lLastCounterR = g_lConnterRImage;
    g_bRunFlagB = TRUE;
    
    return FALSE;
}

/***********************************************************
*   函数说明：实数求模函数                                 *
*   输入：    被除数，除数                                 *
*   输出：    取模结果                                     *
*   调用函数：无                                           *
***********************************************************/
float fMod(float fNumberA,float fNumberB)
{
    if (fNumberB < 0)
    {
        if (fNumberA < 0)
        {
            while(fNumberA < fNumberB)
            {
                fNumberA -= fNumberB;
            }
        
            return fNumberA;
        }
        else if (fNumberA > 0)
        {
            while((fNumberA + fNumberB) > fNumberB)
            {
                fNumberA += fNumberB;
            }
            
            return fNumberA;
        }
        else
        {
            return 0.0;
        }
    }
    else if (fNumberB > 0)
    {
        if (fNumberA < 0)
        {
            while((fNumberA + fNumberB) < fNumberB)
            {
                fNumberA += fNumberB;
            }
            
            return fNumberA;
        }
        else if (fNumberA > 0)
        {
            while(fNumberA > fNumberB)
            {
                fNumberA -= fNumberB;
            }
            
            return fNumberA;
        }
        else
        {
            return 0.0;
        }
    }
    else
    {
        return 0.0;
    }
}