private void RefreshProductOuthouse()
{
switch(_workStatus)
{
case MachineWorkStatus.MACHINE_IDLE:
{
_TimeElapse = 0; //计时清零
_workStatus = MachineWorkStatus.PRODUCT_OUT_PREPARE;
break;
}
case MachineWorkStatus.PRODUCT_OUT_PREPARE:
{
_TimeElapse += (int)(MotionTimerInterval * SimClockRate);
int CellMoveMax = _TimeElapse / _CellMoveTime; //最多能移动格数
if (Math.Abs(_TargetCoord.L - _CurrentCoord.L) < CellMoveMax)
{
_CurrentCoord.L = _TargetCoord.L;
int CellMoveColumnMax = CellMoveMax - Math.Abs(_TargetCoord.L - _CurrentCoord.L);
if (Math.Abs(_TargetCoord.C - _CurrentCoord.C) < CellMoveColumnMax)
{
_CurrentCoord.C = _TargetCoord.C;
_workStatus = MachineWorkStatus.PRODUCT_OUT_LOADED;
_TimeElapse = 0; //计时清零
}
else
{
_TimeElapse -= (int)(CellMoveMax * MotionTimerInterval * SimClockRate);
if (_TargetCoord.C > _CurrentCoord.C)
{
_CurrentCoord.C += CellMoveColumnMax;
}
else
{
_CurrentCoord.C -= CellMoveColumnMax;
}
}
}
else
{
_TimeElapse -= (int)(CellMoveMax * MotionTimerInterval * SimClockRate);
if (_TargetCoord.L > _CurrentCoord.L)
{
_CurrentCoord.L += CellMoveMax;
}
else
{
_CurrentCoord.L -= CellMoveMax;
}
}
break;
}
case MachineWorkStatus.PRODUCT_OUT_LOADED:
{
_TimeElapse += (int)(MotionTimerInterval * SimClockRate);
if (_TimeElapse >= _LoadTime)
{
_workStatus = MachineWorkStatus.PRODUCT_OUTCOMING;
_TimeElapse = 0; //计时器清零
}
break;
}
case MachineWorkStatus.PRODUCT_OUTCOMING:
{
CellPos targetPos = new CellPos();
targetPos.L = 1;
targetPos.C = 50;
_TimeElapse += (int)(MotionTimerInterval * SimClockRate);
int CellMoveMax = _TimeElapse / _CellMoveTime; //最多能移动格数
//先移动到目标层，再移动到目标列
if (Math.Abs(targetPos.L - _CurrentCoord.L) < CellMoveMax)
{
_CurrentCoord.L = targetPos.L;
int CellMoveColumnMax = CellMoveMax - Math.Abs(targetPos.L - _CurrentCoord.L);
if (Math.Abs(targetPos.C - _CurrentCoord.C) < CellMoveColumnMax)
{
_CurrentCoord.C = targetPos.C;
_workStatus = MachineWorkStatus.PRODUCT_OUT_UNLOADED;
_TimeElapse = 0; //计时清零
}
else
{
_TimeElapse -= (int)(CellMoveMax * MotionTimerInterval * SimClockRate);
if (targetPos.C > _CurrentCoord.C)
{
_CurrentCoord.C += CellMoveColumnMax;
}
else
{
_CurrentCoord.C -= CellMoveColumnMax;
}
}
}
else
{
_TimeElapse -= (int)(CellMoveMax * MotionTimerInterval * SimClockRate);
if (targetPos.L > _CurrentCoord.L)
{
_CurrentCoord.L += CellMoveMax;
}
else
{
_CurrentCoord.L -= CellMoveMax;
}
}
break;
}
case MachineWorkStatus.PRODUCT_OUT_UNLOADED:
{
_TimeElapse += (int)(MotionTimerInterval * SimClockRate);
if(_TimeElapse>= _UnloadTime)
{
_workStatus = MachineWorkStatus.PRODUCT_OUTHOUSE_OK;
_TimeElapse = 0;
}
break;
}
case MachineWorkStatus.PRODUCT_OUTHOUSE_OK:
{
_workStatus = MachineWorkStatus.MACHINE_IDLE;
_CurrentCmd = 0;
_TimeElapse = 0;
break;
}
}
}
/// <summary>
/// 接收udp数据包，更新状态
/// </summary>
/// <param name="package"></param>
public void RecvCmd( udpPackage package,string requestIP,int requestPort)
{
lock(this)
{
_requestIP = requestIP;
_requestPort = requestPort;
if (package.frameType == 2)
return;
if (package.addr != _MachineIndex)
return;
switch (package.cmd)
{
case (byte)HouseCmdCode.CMD_QUERY:
{
udpPackage rePackage = new udpPackage();
rePackage.len = 0x09;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = (byte)HouseCmdCode.CMD_QUERY;
rePackage.status = 0;
rePackage.data = new byte[5];
rePackage.data[0] = (byte)_CurrentCoord.L;
rePackage.data[1] = (byte)(_CurrentCoord.C & 0xff);
rePackage.data[2] = (byte)((_CurrentCoord.C >> 8) & 0xff);
rePackage.data[3] = (byte)_workStatus;
rePackage.data[4] = 0x00;
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
break;
}
case (byte)HouseCmdCode.CMD_PRODUCT_INHOUSE:
{
if (_workStatus == MachineWorkStatus.MACHINE_IDLE)
{
//如小车空闲则接收该指令
_workStatus = MachineWorkStatus.PRODUCT_IN_PREPARE;
_CurrentCmd = (HouseCmdCode)package.cmd;
_TargetCoord.L = package.data[0];
_TargetCoord.R = package.data[1];
_TargetCoord.C = package.data[2] + (package.data[3] << 8);
//应答
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x00;
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
_svc.RefreshTaskDisp(this.MachineIndex, _TargetCoord, "入库");
}
else
{
//应答，由于小车正在作业，该指令拒绝接收
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x01; //拒绝执行
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
break;
}
case (byte)HouseCmdCode.CMD_PRODUCT_OUTHOUSE:
{
if (_workStatus == MachineWorkStatus.MACHINE_IDLE)
{
//如小车空闲则接收该指令
_workStatus = MachineWorkStatus.PRODUCT_OUT_PREPARE;
_CurrentCmd = (HouseCmdCode)package.cmd;
_TargetCoord.L = package.data[0];
_TargetCoord.R = package.data[1];
_TargetCoord.C = package.data[2] + (package.data[3] << 8);
//应答
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x00;
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
_svc.RefreshTaskDisp(this.MachineIndex, _TargetCoord, "出库");
}
else
{
//应答，由于小车正在作业，该指令拒绝接收
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x01; //拒绝执行
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
break;
}
case (byte)HouseCmdCode.CMD_PALLETE_INHOUSE:
{
if (_workStatus == MachineWorkStatus.MACHINE_IDLE)
{
//如小车空闲则接收该指令
_workStatus = MachineWorkStatus.PALLETE_IN_PREPARE;
_CurrentCmd = (HouseCmdCode)package.cmd;
_TargetCoord.L = package.data[0];
_TargetCoord.R = package.data[1];
_TargetCoord.C = package.data[2] + (package.data[3] << 8);
//应答
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x00;
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
else
{
//应答，由于小车正在作业，该指令拒绝接收
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x01; //拒绝执行
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
break;
}
case (byte)HouseCmdCode.CMD_PALLETE_OUTHOUSE:
{
if (_workStatus == MachineWorkStatus.MACHINE_IDLE)
{
//如小车空闲则接收该指令
_workStatus = MachineWorkStatus.PALLETE_OUT_PREPARE;
_CurrentCmd = (HouseCmdCode)package.cmd;
_TargetCoord.L = package.data[0];
_TargetCoord.R = package.data[1];
_TargetCoord.C = package.data[2] + (package.data[3] << 8);
//应答
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x00;
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
else
{
//应答，由于小车正在作业，该指令拒绝接收
udpPackage rePackage = new udpPackage();
rePackage.len = 0x06;
rePackage.frameType = 0x02;
rePackage.addr = (byte)_MachineIndex;
rePackage.cmd = package.cmd;
rePackage.status = 0x01; //拒绝执行
_svc.SendRemsg(rePackage, _requestIP, _requestPort);
}
break;
}
default:
break;
}
}
}
