public TransMachineControl(int machineNo,string plcIP,int plcPort,int localPort)
{
MachineNo = MachineNo;
_plcIp = plcIP;
_plcPort = plcPort;
_localPort = localPort;
MonitoringInterval = 500;
MStatus = MachineWorkStatus.MACHINE_IDLE;
}
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;
}
}
}
public void DispMachineStatus(int MachineIndex,MachineWorkStatus workStatus)
{
this.Invoke(new delegateRefreshUI(ThreadUIDispMachineStatus), MachineIndex, _MachineStatusDic[(int)workStatus]);
}
/// <summary>
/// 接收数据线程函数
/// </summary>
private void recvPlcProc()
{
byte[] data = new byte[1024];
while (_conPlc)
{
int recv =0;
byte[] byteStream = null;
try
{
// recv= _sockPlc.ReceiveFrom(data, ref _epPlc);
byteStream = _udpClient.Receive(ref C_Point);
_heartCount = 0; //收到数据包，清零，标识连接建立
_bConnected = true;
}
catch (System.Exception e)
{
Console.WriteLine("plc comm:"+e.Message);
continue;
}
//事件参数
udpPackage package = new udpPackage();
for (int i = 0; i < recv; i++)
{
byteStream[i] = data[i];
}
if (package.Pack(byteStream))
{
if ((package.frameType == 0x02) && (package.addr == this.MachineNo))
{
//应答帧,地址正确
if (package.cmd == (byte)HouseCmdCode.CMD_QUERY)
{
MachineWorkStatus newS = (MachineWorkStatus)package.data[3];
TMCoord newPos = new TMCoord(package.data[0], this.MachineNo, (package.data[1] + (package.data[2] << 8)));
if (newS != MStatus)
{
if(MStatus == MachineWorkStatus.PRODUCT_INHOUSE_OK)
{
string ss = "入库任务"+(_currentCmdIndex+1).ToString()+"完成";
Console.WriteLine(ss);
}
MStatus = newS;
// string strS = "小车"+MachineNo.ToString()+"状态:"+MStatus.ToString();
// Console.WriteLine(strS);
msgTMEventArg e = new msgTMEventArg();
e.MachineNo = this.MachineNo;
e.ErrorHappened = false;
e.TMStatus = newS;
if (EventTMStatusUpdated != null)
{
EventTMStatusUpdated(this, e);
}
}
if(newPos != this.currentCoord)
{
msgTMEventArg e = new msgTMEventArg();
e.MachineNo = this.MachineNo;
e.ErrorHappened = false;
e.TMPos = newPos;
if(EventTMPosUpdated != null)
{
EventTMPosUpdated(this,e);
}
}
}
else if (package.cmd == (byte)HouseCmdCode.CMD_PRODUCT_INHOUSE)
{
}
else if (package.cmd == (byte)HouseCmdCode.CMD_PRODUCT_OUTHOUSE)
{
}
}
}
}
}
