2019年11月06日 | 单片机全场定位系统程序

接线说明：
//左编码器
PD3   INT1
PB0   PCINT0

//右编码器
PD2   INT0
PC3   PCINT11

/***********************************************************

*   函数库说明：差分定位数值计算函数库                     *

*   版本：      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_lCounterRImage;

    g_bRunFlagB = TRUE;

    return FALSE;

}

/***********************************************************

*   函数说明：实数求模函数                                 *