/// <summary>
///小车移动函数，小车的横纵坐标每改变一次，窗体就执行重绘函数，就实现了小车的移动
/// </summary>
/// <param name="Elc"></param>
public int SearchRoute(ElecMap Elc, int startX, int startY, int endX, int endY)//04022
{
AstarSearch astartSearch = new AstarSearch();
Elc.mapnode[startX, startY].nodeCanUsed = true; //小车自己所在的地方解除占用
Elc.TempMapNode[startX, startY].nodeCanUsed = true; //小车自己所在的地方解除占用
this.route = astartSearch.search(Elc, Elc.Width, Elc.Height, startX, startY, endX, endY);
routeIndex = 0;
if (this.route == null)
{
this.vehical_state = v_state.cannotToDestination;
}
for (int ii = 0; ii < this.route.Count; ii++)
{
this.cost = this.cost + 1;
// Elc.mapnode[(int)this.route[ii].X, (int)this.route[ii].Y].vehiclePriority = new List<int>();
// Elc.mapnode[(int)this.route[ii].Height, (int)this.route[ii].Width].vehiclePriority.Add(this.v_num);
}
//解除节点的占用
for (int p = 0; p < Elc.heightNum; p++)
{
for (int q = 0; q < Elc.widthNum; q++)
{
Elc.mapnode[p, q].nodeCanUsed = true;
Elc.TempMapNode[p, q].nodeCanUsed = true;//小车自己所在的地方解除占用
}
}
return(this.route.Count);
}
/// <summary>
/// 重绘函数
/// </summary>
/// <param name="g"></param>
public void Draw(Graphics g, ElecMap Elc)
{
if (route.Count == 0)
{
g.DrawImage(this.V_Picture, this.X, this.Y);
}
else
{
Elc.TempMapNode[(int)route[routeIndex].y, (int)route[routeIndex].x].nodeCanUsed = false;
g.DrawImage(this.V_Picture, route[routeIndex].y * constDefine.BENCHMARK + constDefine.BEGIN_X, route[routeIndex].x * constDefine.BENCHMARK);
}
}
public void Vehicle_Move(ElecMap Elc)
{
if (route.Count > 0)
{
if (routeIndex < this.route.Count - 1)
{
routeIndex++;
}
else
{
this.Arrive = true;
//if ((this.Y / constDefine.BENCHMARK) == this.endX && (this.X / constDefine.BENCHMARK) ==this.endY)
//{
// Elc.TempMapNode[this.endX, this.endY].oth = Elc.mapnode[this.endX, this.endY].oth;
//}
}
}
}
void InitUiView(string path)
{
Elc = new ElecMap();
Elc.InitialElc(path);
this.WindowState = FormWindowState.Maximized;
this.tableLayoutPanel1.ColumnStyles[0].Width = (int)FORM_WIDTH * (1 - PANEL_RADIO) / 2;
this.tableLayoutPanel1.ColumnStyles[1].Width = (int)FORM_WIDTH * (PANEL_RADIO);
this.tableLayoutPanel1.ColumnStyles[2].Width = (int)FORM_WIDTH * (1 - PANEL_RADIO) / 2;
ConstDefine.g_NodeLength = (int)(FORM_WIDTH * PANEL_RADIO) / ConstDefine.g_WidthNum;
MAX_NODE_LENGTH = ConstDefine.g_NodeLength * 3;
MIN_NODE_LENGTH = ConstDefine.g_NodeLength * 2 / 3;
////设置滚动条滚动的区域
//this.AutoScrollMinSize = new Size(ConstDefine.WIDTH + ConstDefine.BEGIN_X, ConstDefine.HEIGHT);
mapControl = new MapControl(Elc);
rowComBox.SelectedIndex = 0;
colComBox.SelectedIndex = 0;
SetMapView();
}
public SearchManager()
{
Elc = ElecMap.Instance;
astarSearch = new AgvPathPlanning();
}
/// <summary>
/// 移动
/// </summary>
/// <param name="Elc"></param>
/// <returns>是否移动了</returns>
public bool Move(ElecMap Elc)
{
lock (RouteLock)
{
if (route == null || route.Count < 1)
{
return(false);
}
if (tPtr >= route.Count - 1)
{
Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id;
Arrive = true;
return(false);
}
if (ShouldMove(tPtr + 1) == false)
{
if (this.WaitEndTime < DateTime.Now)//超过等待时间还不能走，则重新发送一下当前位置
{
Console.WriteLine("Resend Current location");
return(true);
}
return(false);
}
bool virtulChange = false;
if (tPtr == 0)// ConstDefine.FORWORD_STEP)
{
for (VirtualTPtr = 1; VirtualTPtr < ConstDefine.FORWORD_STEP; VirtualTPtr++)
{
if (tPtr + VirtualTPtr <= route.Count - 1)
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
// Boolean temp = Elc.canMoveToNode(this, tx, ty);
int temp = -1;// Elc.mapnode[tx, ty].NodeCanUsed;
if (temp > -1)
{
Stoped = temp;
StopTime--;
return(false);
}
else
{
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
}
}
}
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
StopTime = ConstDefine.STOP_TIME;
tPtr++;
}
else if (tPtr > 0)
{
if (VirtualTPtr <= route.Count - 1)
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
// Boolean temp = Elc.canMoveToNode(this, tx, ty);
int temp = -1;// Elc.mapnode[tx, ty].NodeCanUsed;
if (temp > -1)
{
Stoped = temp;
StopTime--;
return(false);
}
else
{
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
StopTime = ConstDefine.STOP_TIME;
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
tPtr++;
VirtualTPtr++;
virtulChange = true;
}
}
else
{
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
StopTime = ConstDefine.STOP_TIME;
tPtr++;
}
}
BeginX = route[tPtr].X;
BeginY = route[tPtr].Y;
return(true);
//if (ShouldMove(route[tPtr].X, route[tPtr].Y))
//{
// BeginX = route[tPtr].X;
// BeginY = route[tPtr].Y;
// return true;
//}
//else
//{
// //int tx = (int)route[VirtualTPtr].X;
// //int ty = (int)route[VirtualTPtr].Y;
// //Elc.mapnode[tx, ty].NodeCanUsed = -1;
// //Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
// //tx = (int)route[VirtualTPtr].X;
// //ty = (int)route[VirtualTPtr].Y;
// //Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
// //Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = this.Id;
// tPtr--;
// if (virtulChange)
// {
// VirtualTPtr--;
// }
// return false;
//}
}
}
/// <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
}
}
/// <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)
{
SetNodeOccpyed(route[route.Count - 1].X, route[route.Count - 1].Y);
Arrive = true;
return(MoveType.arrived);
if (EqualWithRealLocation(route[route.Count - 1].X, route[route.Count - 1].Y))
{
Arrive = true;
return(MoveType.arrived);
}
else
{
if (agvInfo.OrderExec == OrderExecState.Free)
{
return(MoveType.move);
}
else
{
return(MoveType.arrived);
}
}
}
#if moni
#else
MoveType checkResult = CheckAgvCorrect();
if (checkResult != MoveType.move)
{
if (checkResult == MoveType.clearFault)
{
if (clearFaultCount < 0)
{
return(MoveType.agvFault);
}
else
{
clearFaultCount--;
return(MoveType.clearFault);
}
}
return(checkResult);
}
clearFaultCount = MAX_CLEAR_FAULT_COUNT;
////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)
{
return(MoveType.cannotReceiveSwerverCommands);
}
#endif
bool canMove = false;
Direction virtualDir = route[TPtr + 1].Dir;
for (VirtualTPtr = TPtr + 1; VirtualTPtr < TPtr + config.ForwordStep; VirtualTPtr++)
{
if (VirtualTPtr <= route.Count - 1)
{
if (!VitrualStepCanMove(VirtualTPtr))
{
break;
}
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;
bool res = SetNodeOccpyed(tx, ty);
if (res == false)
{
break;
}
if (agvInfo.OrderExec != OrderExecState.Run)
{
break;
}
}
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
StopTime = ConstDefine.STOP_TIME;
TPtr++;
BeginX = route[TPtr].X;
BeginY = route[TPtr].Y;
return(MoveType.move);
#endif
}
else
{
StopTime--;
return(MoveType.stopAvoidConflict);
}
}
}
/// <summary>
/// 移动
/// </summary>
/// <param name="Elc"></param>
/// <returns>是否移动了</returns>
public bool Move(ElecMap Elc)
{
lock (RouteLock)
{
//if (BeginX == EndX && BeginY == EndY)
//{
// Elc.mapnode[BeginX, BeginY].NodeCanUsed = this.v_num;
// Arrive = true;
// return;
//}
if (route == null || route.Count < 1)
{
return(false);
}
if (tPtr == 0)// ConstDefine.FORWORD_STEP)
{
for (VirtualTPtr = 1; VirtualTPtr < ConstDefine.FORWORD_STEP; VirtualTPtr++)
{
if (tPtr + VirtualTPtr <= route.Count - 1)
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
int temp = -1;// Elc.mapnode[tx, ty].NodeCanUsed;
if (temp > -1)
{
Stoped = temp;
StopTime--;
return(false);
}
else
{
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
}
}
}
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
StopTime = ConstDefine.STOP_TIME;
tPtr++;
}
else if (tPtr > 0)
{
if (VirtualTPtr <= route.Count - 1)
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
int temp = -1;// Elc.mapnode[tx, ty].NodeCanUsed;
if (temp > -1)
{
Stoped = temp;
StopTime--;
return(false);
}
else
{
//if (ShouldMove(route[tPtr + 1].X, route[tPtr + 1].Y))
//{
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
StopTime = ConstDefine.STOP_TIME;
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
tPtr++;
VirtualTPtr++;
//}
//else
//{
// return false;
//}
}
}
else
{
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
StopTime = ConstDefine.STOP_TIME;
tPtr++;
}
}
//BeginX = route[tPtr].X;
//BeginY = route[tPtr].Y;
//return true;
if (tPtr == 3)
{
int a = 0;
}
if (tPtr >= route.Count)
{
Elc.mapnode[route[route.Count - 1].X, route[route.Count - 1].Y].NodeCanUsed = this.Id;
BeginX = route[route.Count - 1].X;
BeginY = route[route.Count - 1].Y;
Arrive = true;
return(true);
}
if (ShouldMove(route[tPtr].X, route[tPtr].Y))
{
BeginX = route[tPtr].X;
BeginY = route[tPtr].Y;
return(true);
}
else
{
int tx = (int)route[VirtualTPtr].X;
int ty = (int)route[VirtualTPtr].Y;
Elc.mapnode[tx, ty].NodeCanUsed = -1;
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = -1;
tPtr--;
VirtualTPtr--;
tx = (int)route[VirtualTPtr].X;
ty = (int)route[VirtualTPtr].Y;
Elc.mapnode[tx, ty].NodeCanUsed = this.Id;
Elc.mapnode[route[tPtr].X, route[tPtr].Y].NodeCanUsed = this.Id;
return(false);
}
}
}
public SearchManager()
{
Elc = ElecMap.Instance;
astarSearch = new AstarSearch();
}
