久久久久日本国产精品,欧美日韩免费在线视频,国产午夜一区二区三区网

/***********************************************************
* 函數(shù)庫說明：差分定位數(shù)值計(jì)算函數(shù)庫 *
* 版本： v1.00beta *
* 作者：王卓然 *
* 創(chuàng)建日期： 2008年3月29日 *
* -------------------------------------------------------- *
* [支持庫] *
* 支持庫名稱： *
* 需要版本： *
* 支持庫說明： *
* -------------------------------------------------------- *
* [版本更新] *
* 修改： *
* 修改日期： *
* 版本： *
* -------------------------------------------------------- *
* [版本歷史] *
* -------------------------------------------------------- *
* [使用說明] *
***********************************************************/
/********************
* 頭文件配置區(qū) *
********************/
# include "RD_MacroAndConst.h"
# include "DL_Config.h"
# include "RD_UseDLocate_LIB.h"
# include "PF_Config.h"
# include "Math.h"
/********************
* 系統(tǒng) 宏定義 *
********************/
/*------------------*
* 常數(shù) 宏定義 *
*------------------*/
//輪子直徑
#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
//碼盤倍頻數(shù)
#ifndef MULTIPLE
#error No define MULTIPLE
#endif
//計(jì)數(shù)器到弧長(zhǎng)
# define K_L ((PI * (WHEEL_D_L)) / ((MULTIPLE) * (N_L)))
# define K_R ((PI * (WHEEL_D_R)) / ((MULTIPLE) * (N_R)))
/*------------------*
* 動(dòng) 作宏定義 *
*------------------*/
/********************
* 自定義類型聲明區(qū) *
********************/
/********************
* 模塊結(jié)構(gòu)體定義區(qū) *
********************/
/********************
* 函數(shù) 聲明區(qū) *
********************/
float Get_Absolute_Angle(void);
float fMod(float fNumberA,float fNumberB);
BOOL PROC_Difference_Locate(void);
/********************
* 模塊函數(shù)聲明區(qū) *
********************/
/********************
* 模塊變量聲明區(qū) *
********************/
/********************
* 全局變量聲明區(qū) *
********************/
float g_fLastAngle = 0.0;
float g_fX = 0.0;
float g_fY = 0.0;
/********************
* 全局變量聲引用區(qū) *
********************/
extern BOOL g_bRunFlagB;
/***********************************************************
* 函數(shù)說明：獲取當(dāng)前絕對(duì)角度函數(shù) *
* 輸入：無 *
* 輸出：角度值 *
* 調(diào)用函數(shù)：無 *
***********************************************************/
float Get_Absolute_Angle(void)
{
return
(
(float)g_lCounterR * (float)K_R
- (float)g_lCounterL * (float)K_L
) * (1.0 / (float)D_BTW_WHEEL);
}
/***********************************************************
* 函數(shù)說明：獲取當(dāng)前相對(duì)角度函數(shù) *
* 輸入：左編碼器微分量，右編碼器微分量 *
* 輸出：角度值（弧度） *
* 調(diào)用函數(shù)：無 *
***********************************************************/
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);
}
/***********************************************************
* 函數(shù)說明：差分定位計(jì)算函數(shù) *
* 輸入：無 *
* 輸出： FALSE *
* 調(diào)用函數(shù)：無 *
***********************************************************/
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;
//計(jì)算角度微元
{
float fTempAngle = Get_Relative_Angle
(
g_lCounterLImage,
g_lCounterRImage
);
fDeltaAngle = (fTempAngle - g_fLastAngle);
fAbsoluteAngle = fDeltaAngle * 0.5 + g_fLastAngle;
g_fLastAngle = fTempAngle;
}
//計(jì)算位置微元
{
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);
}
}
//數(shù)據(jù)更新
s_lLastCounterL = g_lCounterLImage;
s_lLastCounterR = g_lCounterRImage;
g_bRunFlagB = TRUE;
return FALSE;
}
/***********************************************************
* 函數(shù)說明：實(shí)數(shù)求模函數(shù) *
* 輸入：被除數(shù)，除數(shù) *
* 輸出：取模結(jié)果 *
* 調(diào)用函數(shù)：無 *
***********************************************************/
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;
}
}

復(fù)制代碼

欧美极品高清xxxxhd,国产日产欧美最新,无码AV国产东京热AV无码,国产精品人与动性XXX,国产传媒亚洲综合一区二区,四库影院永久国产精品,毛片免费免费高清视频,福利所导航夜趣136