coderbaseposepredict.cpp

InnovLabSimu在vc++下实现

#include "CoderBasePosePredict.h"


CCoderBasePosePredict::CCoderBasePosePredict(void)
{
}

CCoderBasePosePredict::~CCoderBasePosePredict(void)
{
}
void  CCoderBasePosePredict::GetSelfOriention(float dl,float dr)
{
	if (dl>dr)//左轮的移动距离>右轮的移动距离朝右拐即角度的变化为负角
	{
		m_fdeltaOrientation=-(dl-dr)/(ROBOT_WHEEL_DIS);//目前单位为弧度
		m_iRightNeg_LeftPosit=-1;
	}
	else if (fabs(dl-dr)<0.001)
	{
		m_fdeltaOrientation=0;
		m_iRightNeg_LeftPosit=0;
	}
	else//左轮的移动距离<右轮的移动距离,朝左拐即角度的变化为正角
	{
		m_fdeltaOrientation=(dr-dl)/(ROBOT_WHEEL_DIS);//目前单位为弧度
		m_iRightNeg_LeftPosit=1;
	}	
}
RobotLoc& CCoderBasePosePredict::PredictNewCoord(float dl,float dr,float OrientNoise)
{
	GetSelfOriention(dl,dr);
	
	if (m_iRightNeg_LeftPosit!=0)
	{
		CenterRadiu=0.5*(dl+dr)*ROBOT_WHEEL_DIS/(dr-dl);
		SelfLoc.XCoord=PreSelfLoc.XCoord-CenterRadiu*(sin(PreSelfLoc.Orientation)-sin(PreSelfLoc.Orientation+m_fdeltaOrientation));
		SelfLoc.YCoord=PreSelfLoc.YCoord-CenterRadiu*(cos(PreSelfLoc.Orientation+m_fdeltaOrientation)-cos(PreSelfLoc.Orientation));
		SelfLoc.Orientation=PreSelfLoc.Orientation+m_fdeltaOrientation+OrientNoise;
	}
	else
	{
		CenterTrackLength=0.5*(dl+dr);///<中心移动的长度
		SelfLoc.Orientation=PreSelfLoc.Orientation;
		SelfLoc.XCoord=PreSelfLoc.XCoord+CenterTrackLength*cos(PreSelfLoc.Orientation);
		SelfLoc.YCoord=PreSelfLoc.YCoord+CenterTrackLength*sin(PreSelfLoc.Orientation);
	}
	LimitAngleIn0_2Pi(SelfLoc.Orientation);
	return SelfLoc;
}