//#include "StdAfx.h"
#include "RobotMathmaticalModel.h"

CRobotMathmaticalModel::CRobotMathmaticalModel(void)
{
}

CRobotMathmaticalModel::~CRobotMathmaticalModel(void)
{
}

// 速度運動模型，用機器人的線速度、角速度計算左右輪速
bool CRobotMathmaticalModel::VelocityMotionModel(float LinearVel, float AngularVel,float RobotWidth)
{
	m_dLeftSpeed=LinearVel+RobotWidth*AngularVel;//5/2=2.5
	m_dRightSpeed=LinearVel-RobotWidth*AngularVel;
	return SafeLRSpeedLimit(m_dLeftSpeed,m_dRightSpeed);
}