/// <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; }