void SetCurrentNodeOccpyAndOldNodeFree() { MyPoint cur = new MyPoint(RealX, RealY, dir); // Console.WriteLine(RealX + "," + RealY); if (null != Route && Route.Count > 0) { int index = 0; for (; index < Route.Count; index++) { if (cur.Equals(Route[index])) { break; } } if (index >= Route.Count) { return; //没有找到 } //Console.WriteLine(index); for (int j = 0; j < index; j++) { ElecMap.Instance.mapnode[Route[j].X, Route[j].Y].NodeCanUsed = -1; } //ElecMap.Instance.mapnode[Route[index].X, Route[index].Y].NodeCanUsed = Id; } }
bool VitrualStepCanMove(int nextTPtr) { MyPoint cur = new MyPoint(realX, realY, dir); int index = 0; for (; index < route.Count; index++) { if (cur.Equals(route[index])) { break; } } if (index >= route.Count - 1) { index = route.Count - 1; } if (nextTPtr >= index + config.ForwordStep)//超过最大探测距离 { return(false); } return(true); }
/// <summary> /// 移动 /// </summary> /// <param name="Elc"></param> /// <returns></returns> public MoveType Move(ElecMap Elc) { lock (RouteLock) { if (route == null || route.Count < 1) { return(MoveType.arrived); } if (TPtr >= route.Count - 1) { Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id; Arrive = true; return(MoveType.arrived); //Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id; //if (EqualWithRealLocation(route[route.Count - 1].X, route[route.Count - 1].Y)) //{ // Arrive = true; // return MoveType.arrived; //} //else //{ // return MoveType.move; //} } #if moni #else if (!CheckAgvCorrect()) { return(MoveType.agvFault); } ////TODO://如果当前方向就是小车方向给它发转弯是否会出错?转向完成,翻盘完成的标志是否要用? //if (!swerverFinished) //{ // return MoveType.cannotReceiveRunCommands; //} //dir = GetAgvDireciton(); //Console.WriteLine(dir.ToString()); //if (TPtr == 0 && route[0].Dir != dir) //{ // if (AgvCanReceiveSwerverCommands()) // { // return MoveType.move; // swerverFinished = false; // return GetSwerveAngle(route[0].Dir); // } // else // { // return MoveType.cannotReceiveSwerverCommands; // } //} //if (route[TPtr + 1].Dir != dir) //{ // if (AgvCanReceiveSwerverCommands()) // { // return MoveType.move; // swerverFinished = false; // return GetSwerveAngle(route[TPtr + 1].Dir); // } // else // { // return MoveType.cannotReceiveSwerverCommands; // } //} if (!AgvCanReceiveRunCommands()) { return(MoveType.cannotReceiveRunCommands); } if (ShouldMove(TPtr + 1) == false) { BeginX = route[TPtr].X; BeginY = route[TPtr].Y; if (this.WaitEndTime < DateTime.Now)//超过等待时间还不能走,则重新发送一下当前位置 { Console.WriteLine("Resend Current location"); return(MoveType.move); } return(MoveType.cannotReceiveRunCommands); } #endif MyPoint cur = new MyPoint(RealX, RealY, dir); int index = 0; for (; index < route.Count; index++) { if (cur.Equals(route[index])) { break; } } if (index >= route.Count - 1) { index = route.Count - config.ForwordStep; } bool canMove = false; Direction virtualDir = route[index + 1].Dir; for (VirtualTPtr = index + 1; VirtualTPtr < index + config.ForwordStep; VirtualTPtr++) { if (VirtualTPtr <= route.Count - 1) { int tx = (int)route[VirtualTPtr].X; int ty = (int)route[VirtualTPtr].Y; Direction tDir = route[VirtualTPtr].Dir; Boolean IsCanMoveTo = Elc.IsVehicleCanMove(this, tx, ty);// Elc.mapnode[tx, ty].NodeCanUsed; if (IsCanMoveTo && virtualDir == tDir) { canMove = true; virtualDir = tDir; Elc.mapnode[tx, ty].NodeCanUsed = this.Id; } else { break; } } } if (canMove) { #if moni //if (TPtr == 0) //{ // ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1; //} ElecMap.Instance.mapnode[BeginX, BeginY].NodeCanUsed = -1; TPtr++; BeginX = route[TPtr].X; BeginY = route[TPtr].Y; #else TPtr++; BeginX = route[TPtr].X; BeginY = route[TPtr].Y; #endif return(MoveType.move); } else { StopTime--; return(MoveType.stopAvoidConflict); } #region old /* * if (TPtr == 0)// config.ForwordStep) * { * * for (VirtualTPtr = 1; VirtualTPtr < config.ForwordStep; VirtualTPtr++) * { * if (TPtr + VirtualTPtr <= route.Count - 1) * { * int tx = (int)route[VirtualTPtr].X; * int ty = (int)route[VirtualTPtr].Y; * int temp = Elc.mapnode[tx, ty].NodeCanUsed; * if (temp > -1) * { * Stoped = temp; * StopTime--; * return false; * } * else * { * Elc.mapnode[tx, ty].NodeCanUsed = this.Id; * } * } * } * StopTime = StopTime; * TPtr++; * * } * else if (TPtr > 0) * { * * if (VirtualTPtr <= route.Count - 1) * { * int tx = (int)route[VirtualTPtr].X; * int ty = (int)route[VirtualTPtr].Y; * int temp = Elc.mapnode[tx, ty].NodeCanUsed; * if (temp > -1) * { * Stoped = temp; * StopTime--; * return false; * } * else * { * Elc.mapnode[tx, ty].NodeCanUsed = this.Id; * StopTime = StopTime; * TPtr++; * VirtualTPtr++; * } * * } * else * { * StopTime = StopTime; * TPtr++; * } * } * BeginX = route[TPtr].X; * BeginY = route[TPtr].Y; * return true; */ #endregion } }