/// <summary>
/// Get the distance between two points
/// </summary>
public double getDis(KeyPoint point)
{
return(this.getDis(point.x, point.y));
}
/// <summary>
/// 执行点到点编码器导航程序
/// </summary>
/// <param name="res"></param>
/// <param name="des"></param>
/// <param name="conPort"></param>
/// <param name="drPort"></param>
private static void P2P(KeyPoint res, KeyPoint des, IConPort conPort, IDrPort drPort)
{
// 设置角度、距离误差范围
double angRound = 0.008;
double disRound = 0.0075;
// 开始调整的角度大小
double angAdjust = 0.2;
// 当前位置点信息
KeyPoint nowPos = drPort.getPosition();
// 按照相对坐标来走
des.x = nowPos.x + des.x - res.x;
des.y = nowPos.y + des.y - res.y;
des.w = nowPos.w + des.w - res.w;
//
// 1.旋转AGV对准方向
//
int rotSpeed = 0;
if (Math.Abs(nowPos.w - des.w) > angAdjust)
{
while (Math.Abs(nowPos.w - des.w) > angRound &&
ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getRotSpeed(des, nowPos, ref rotSpeed);
// 执行转弯
conPort.Control_Move_By_Speed(0, 0, rotSpeed);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
}
}
//
// 2.前进至目标位置
//
int goSpeed = 0;
int shSpeed = 0;
if (Math.Abs(res.y - des.y) < Math.Abs(res.x - des.x))
{
while (Math.Abs(des.x - nowPos.x) > disRound && ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
// 执行行进控制
conPort.Control_Move_By_Speed(goSpeed, shSpeed, 0);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
Console.WriteLine(nowPos.x);
}
}
else
{
while (Math.Abs(des.y - nowPos.y) > disRound && ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
// 执行行进控制
conPort.Control_Move_By_Speed(goSpeed, shSpeed, 0);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
}
}
}
public KeyPoint(KeyPoint p)
{
this.x = p.x;
this.y = p.y;
this.w = p.w;
}
