单片机全场定位系统程序小车静止或移动过程中码盘进行全场定位

ID:102668 · 发表于 2016-1-10 23:04

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

接线说明：
//左编码器
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;
}
/***********************************************************
* 函数说明：实数求模函数 *
* 输入：被除数，除数 *
* 输出：取模结果 *
* 调用函数：无 *
***********************************************************/
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;
}
}

复制代码

ID:323568 · 发表于 2018-5-7 17:19

你好，我想问一下就是两个编码器轮子固定的位置是左右平行180度那种吗

帐号		自动登录	找回密码
密码			立即注册

单片机全场定位系统程序 小车静止或移动过程中码盘进行全场定位

相关帖子

单片机全场定位系统程序小车静止或移动过程中码盘进行全场定位