586 lines
22 KiB
C#
586 lines
22 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.ComponentModel;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
using System.Collections;
|
|
using COMM;
|
|
using AR;
|
|
|
|
namespace arDev
|
|
{
|
|
public partial class Narumi : arRS232
|
|
{
|
|
Hashtable SystemCheck, ErrorCheck;
|
|
private Queue Errlog; // 에러발생시 코드 임시 저장용(쓰레드 동기화용)
|
|
|
|
|
|
public int nBatteryNo { get; set; } = 0;
|
|
|
|
|
|
public Narumi()
|
|
{
|
|
SystemCheck = new Hashtable();
|
|
SystemCheck.Add(15, "100");
|
|
SystemCheck.Add(14, "9");
|
|
SystemCheck.Add(13, "8");
|
|
SystemCheck.Add(12, "53");
|
|
SystemCheck.Add(11, "3");
|
|
//SystemCheck.Add(10, "66");
|
|
//SystemCheck.Add(9, "56");
|
|
SystemCheck.Add(8, "6");
|
|
SystemCheck.Add(7, "5");
|
|
SystemCheck.Add(6, "60");
|
|
SystemCheck.Add(5, "1");
|
|
SystemCheck.Add(4, "2");
|
|
//SystemCheck.Add(3, "65");
|
|
//SystemCheck.Add(2, "55");
|
|
//SystemCheck.Add(1, "11");
|
|
//SystemCheck.Add(0, "22");
|
|
|
|
ErrorCheck = new Hashtable();
|
|
ErrorCheck.Add(15, "17");
|
|
ErrorCheck.Add(14, "15");
|
|
ErrorCheck.Add(13, "18");
|
|
ErrorCheck.Add(12, "19");
|
|
//ErrorCheck.Add(11, "300");
|
|
ErrorCheck.Add(10, "299");
|
|
ErrorCheck.Add(9, "298");
|
|
ErrorCheck.Add(8, "297");
|
|
ErrorCheck.Add(7, "296");
|
|
//ErrorCheck.Add(6,"");
|
|
ErrorCheck.Add(5, "12");
|
|
ErrorCheck.Add(4, "13");
|
|
ErrorCheck.Add(3, "14");
|
|
ErrorCheck.Add(2, "16");
|
|
ErrorCheck.Add(1, "11");
|
|
ErrorCheck.Add(0, "10");
|
|
|
|
Errlog = new Queue();
|
|
MinRecvLength = 4;
|
|
|
|
}
|
|
|
|
|
|
|
|
#region "External Events"
|
|
|
|
|
|
/// <summary>
|
|
/// 분석완료된 데이터 이벤트
|
|
/// </summary>
|
|
public event EventHandler<DataEventArgs> DataReceive;
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
protected override bool CustomParser(byte[] buf, out byte[] remainBuffer)
|
|
{
|
|
List<byte> remain = new List<byte>();
|
|
|
|
//데이터는 총 34byt 이며 , DD~77로 구성됨
|
|
Boolean bComplete = false;
|
|
for (int i = 0; i < buf.Length; i++)
|
|
{
|
|
var incomByte = buf[i];
|
|
if (bComplete == true)
|
|
{
|
|
remain.Add(incomByte);
|
|
}
|
|
else
|
|
{
|
|
if (incomByte == 0x02)
|
|
{
|
|
findSTX = true;
|
|
tempBuffer.Clear();
|
|
tempBuffer.Add(incomByte);
|
|
}
|
|
else if (incomByte == 0x03)
|
|
{
|
|
//데이터가 맞게 수신됨
|
|
//tempBuffer.Add(incomByte);
|
|
tempBuffer.Add(incomByte);
|
|
findSTX = false;
|
|
bComplete = true;
|
|
}
|
|
else
|
|
{
|
|
tempBuffer.Add(incomByte);
|
|
if (tempBuffer.Count == 150) //data overload
|
|
{
|
|
findSTX = false;
|
|
tempBuffer.Clear();
|
|
bComplete = false;
|
|
RaiseMessage(MessageType.Error, "Buffer Over");
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
remainBuffer = remain.ToArray();
|
|
return bComplete;
|
|
}
|
|
|
|
public class Dataframe
|
|
{
|
|
public byte STX { get; private set; }
|
|
public byte ETX { get; private set; }
|
|
public byte[] Data { get; private set; }
|
|
public string DataString { get; private set; }
|
|
public byte[] checksum { get; private set; } = new byte[2];
|
|
public bool Valid { get; private set; } = false;
|
|
public string Message { get; private set; } = string.Empty;
|
|
public byte[] Buffer { get; private set; }
|
|
public string Cmd { get; private set; } = string.Empty;
|
|
|
|
public bool Parse(byte[] data, int MinRecvLength = 0)
|
|
{
|
|
if (data == null || data.Any() == false)
|
|
{
|
|
this.Message = string.Format("수신 데이터가 없습니다");
|
|
return false;
|
|
}
|
|
else if (data.Length < 5)
|
|
{
|
|
this.Message = $"데이터의 길이가 5보다 작습니다 길이={data.Length}";
|
|
return false;
|
|
}
|
|
else if (MinRecvLength > 0 && data.Length < MinRecvLength)
|
|
{
|
|
this.Message = $"데이터의 길이가 {MinRecvLength}보다 작습니다 길이={data.Length}";
|
|
return false;
|
|
}
|
|
else if (data[0] != 0x02 || data[data.Length - 1] != 0x03)
|
|
{
|
|
this.Message = $"STX/ETX Error";
|
|
return false;
|
|
}
|
|
Buffer = new byte[data.Length];
|
|
Array.Copy(data, Buffer, data.Length);
|
|
STX = data[0];
|
|
ETX = data[data.Length - 1];
|
|
Array.Copy(data, data.Length - 3, checksum, 0, 2);
|
|
|
|
Data = new byte[data.Length - 4];
|
|
Array.Copy(data, 1, Data, 0, data.Length - 4);
|
|
|
|
if (data.Length > 2) Cmd = System.Text.Encoding.Default.GetString(Data, 0, 3);
|
|
|
|
this.DataString = System.Text.Encoding.Default.GetString(Data);
|
|
|
|
|
|
Valid = true;
|
|
return true;
|
|
}
|
|
public Dataframe(byte[] buffer = null, int minlen = 0)
|
|
{
|
|
if (buffer != null) Parse(buffer);
|
|
}
|
|
}
|
|
|
|
public override bool ProcessRecvData(byte[] data)
|
|
{
|
|
//LastReceiveBuffer
|
|
var frame = new Dataframe(data, MinRecvLength);
|
|
if (frame.Valid == false)
|
|
{
|
|
RaiseMessage(arRS232.MessageType.Error, frame.Message);
|
|
return false;
|
|
}
|
|
else if (frame.DataString.StartsWith("$") == false && CheckSum(data) == false)
|
|
{
|
|
RaiseMessage(arRS232.MessageType.Error, "Checksum Error MSG=" + frame.DataString);
|
|
return false;
|
|
}
|
|
|
|
var retval = true;
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
if (frame.Cmd.Equals("ACK") || frame.Cmd.Equals("NAK"))
|
|
{ // 응답확인값 수신 : Header(ACX), Length(9), CheckSum 확인
|
|
RevACK(frame);
|
|
}
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
else if (frame.Cmd.Equals("STS"))
|
|
{ // AGV 상태 정보 : Header(STS), Length(30), CheckSum 확인
|
|
RevSTS(frame);
|
|
}
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
else if ((frame.Cmd.Equals("TAG") || frame.Cmd.Equals("CAL") || frame.Cmd.Equals("CCA")))
|
|
{ // TAG ID 정보 : Header(STX), Length(13), CheckSum 확인
|
|
RevTAG(frame);
|
|
}
|
|
else if (frame.DataString.StartsWith("$"))
|
|
{
|
|
// $로 시작되는 AGV 상태 표시
|
|
//var text_Sts_Etc = Encoding.Default.GetString(bRcvData, 3, bRcvData.Length - 2).TrimStart(' '); //20210311 김정만 - SmartX FrameWork 사용 안함으로 주석처리
|
|
//var sMessageOther = Encoding.Default.GetString(bRcvData, 3, bRcvData.Length - 2).TrimStart(' ');
|
|
RaiseMessage(arRS232.MessageType.Normal, "$메세지수신:" + frame.DataString);
|
|
}
|
|
else
|
|
{
|
|
try
|
|
{
|
|
DataReceive?.Invoke(this, new DataEventArgs(DataType.UNKNOWN));
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RaiseMessage(arRS232.MessageType.Error, ex.Message);
|
|
retval = false;
|
|
}
|
|
}
|
|
return retval;
|
|
|
|
|
|
}
|
|
|
|
#region [수신] ACK(응답확인값) 분석
|
|
|
|
public string ACKData { get; set; } = string.Empty;
|
|
public DateTime ACKtime { get; set; } = DateTime.Now;
|
|
private void RevACK(Dataframe frame)
|
|
{
|
|
//ACKSADA7
|
|
var rcvdNow = frame.DataString;
|
|
var bRcvData = frame.Buffer;
|
|
|
|
ACKData = rcvdNow.Substring(3);
|
|
ACKtime = DateTime.Now;
|
|
|
|
if (rcvdNow.StartsWith("ACK"))
|
|
{
|
|
//RaiseMessage("get ack");
|
|
DataReceive?.Invoke(this, new DataEventArgs(DataType.ACK));
|
|
//if (datamanager.commandQueue.Count != 0)
|
|
//{
|
|
// byte[] bCmdData = encoding.GetBytes((String)datamanager.commandQueue.Peek());
|
|
// if (bCmdData[1] == bRcvData[4] && bCmdData[2] == bRcvData[5] && bCmdData[3] == bRcvData[6]) //보낸값이 맞는지 확인후 Dequeue
|
|
// {
|
|
// nSendCount = 0;
|
|
// datamanager.commandQueue.Dequeue();
|
|
// sSendCommandString = "";
|
|
// }
|
|
//}
|
|
}
|
|
else if (rcvdNow.StartsWith("NAK"))
|
|
{
|
|
//RaiseMessage("get nak");
|
|
DataReceive?.Invoke(this, new DataEventArgs(DataType.NAK));
|
|
//nSendCount = 0; //NAK - 재전송이 날아왔을때 nSendCommandCounter 초기화
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
public SystemFlag0 system0 = new SystemFlag0();
|
|
public SystemFlag1 system1 = new SystemFlag1();
|
|
public ErrorFlag error = new ErrorFlag();
|
|
public AgvData data = new AgvData();
|
|
public Signal signal = new Signal();
|
|
|
|
#region [수신] STS(AGV상태정보) 분석
|
|
public string LastSTS { get; set; } = string.Empty;
|
|
private void RevSTS(Dataframe frame)
|
|
{
|
|
LastSTS = frame.DataString;
|
|
string rcvdNow = frame.DataString;
|
|
byte[] bRcvData = frame.Buffer;
|
|
var encoding = System.Text.Encoding.Default;
|
|
try
|
|
{
|
|
|
|
// AGV 베터리 잔량 표시 flag (4~6) /////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
var idx = 3;
|
|
var battery = int.Parse(rcvdNow.Substring(idx, 3)) / 10f;// Convert.ToDouble(encoding.GetString(rcvdNow, 3, 3)) / 10;
|
|
idx += 3;
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// AGV 장치연결상태 flag (7~10) - System Flag 0
|
|
var nDataTemp = Convert.ToInt16(rcvdNow.Substring(idx, 4), 16);
|
|
system0.SetValue(nDataTemp);
|
|
idx += 4;
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// AGV Sts(현재 상태) flag (11~14) - System Flag 1
|
|
nDataTemp = Convert.ToInt16(rcvdNow.Substring(idx, 4), 16);
|
|
system1.SetValue(nDataTemp);
|
|
idx += 4;
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
// AGV Error flag (15~18) - Error Flag
|
|
nDataTemp = Convert.ToInt16(rcvdNow.Substring(idx, 4), 16);
|
|
error.SetValue(nDataTemp);
|
|
idx += 4;
|
|
|
|
|
|
data.Speed = rcvdNow.Substring(idx, 1)[0]; idx += 1; //L,M.H
|
|
data.Sts = rcvdNow.Substring(idx, 1)[0]; idx += 1; //S(직진),L(좌분기),R(우분기)
|
|
data.Direction = rcvdNow.Substring(idx, 1)[0]; idx += 1; //F,B,L,R
|
|
data.guidesensor = int.Parse(rcvdNow.Substring(idx, 1)); idx += 1; //가이드 좌측부터 1~9
|
|
|
|
nDataTemp = Convert.ToByte(rcvdNow.Substring(idx, 2), 16);
|
|
signal.SetValue(nDataTemp);
|
|
|
|
//data.Sts = encoding.GetString(bRcvData, 19, 3); //20210311 김정만 - SmartX FrameWork 사용 안함으로 주석처리
|
|
|
|
//var Sts_cSpeed = encoding.GetString(bRcvData, 19, 1)[0];
|
|
//var Sts_cDirection = encoding.GetString(bRcvData, 20, 1)[0];
|
|
//var Sts_cFB = encoding.GetString(bRcvData, 21, 1)[0];
|
|
|
|
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
////가이드센서 정보 (22)//////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//var Sts_nGuide = 0;
|
|
//if (bRcvData[22] > 47 && bRcvData[22] < 58) { Sts_nGuide = Convert.ToInt32(encoding.GetString(bRcvData, 22, 1)); }
|
|
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
////마크센서 & 포토센서 정보 (23~24)////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//nDataTemp = Convert.ToInt32(encoding.GetString(bRcvData, 23, 2), 16);
|
|
|
|
//data.Sts_bMark1 = Convert.ToBoolean(nDataTemp & 0x4);
|
|
//data.Sts_bMark2 = Convert.ToBoolean(nDataTemp & 0x8);
|
|
//data.Sts_bCargo = Convert.ToBoolean(nDataTemp & 0x10);
|
|
|
|
////포토센서
|
|
//if (Sts_cFB == 'F')
|
|
//{
|
|
// system.Sts_nSenser = Convert.ToInt32(encoding.GetString(bRcvData, 26, 1));
|
|
//}
|
|
//else if (Sts_cFB == 'B')
|
|
//{
|
|
// system.Sts_nSenser = Convert.ToInt32(encoding.GetString(bRcvData, 27, 1));
|
|
//}
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
//AGV 속도/분기/방향 (19~21)//////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
|
|
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
|
DataReceive?.Invoke(this, new DataEventArgs(DataType.STS));
|
|
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
RaiseMessage(MessageType.Error, $"Parse Error(STS) = {ex.Message}");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#region [수신] TAG(태그정보) 분석
|
|
|
|
System.Text.Encoding encoding = System.Text.Encoding.Default;
|
|
string old_TagString = string.Empty;
|
|
string old_CALString = string.Empty;
|
|
string old_CCAString = string.Empty;
|
|
private void RevTAG(Dataframe frame)
|
|
{
|
|
string rcvdNow = frame.DataString;
|
|
byte[] bRcvData = frame.Buffer;
|
|
if (rcvdNow.StartsWith("TAG"))
|
|
{
|
|
//221123 chi 숫자로변경
|
|
var tagnostr = rcvdNow.Substring(3);
|
|
if (ushort.TryParse(tagnostr, out ushort tagnoint))
|
|
{
|
|
var Changed = !old_TagString.Equals(tagnostr);
|
|
data.TagString = tagnostr;
|
|
data.TagNo = tagnoint;
|
|
old_TagString = tagnostr;
|
|
DataReceive?.Invoke(this, new DataEventArgs(DataType.TAG));
|
|
}
|
|
|
|
WriteData(MakeCheckSum("ACKTAG"));
|
|
|
|
}
|
|
else if (rcvdNow.StartsWith("CAL"))
|
|
{
|
|
|
|
var tagnostr = encoding.GetString(bRcvData, 4, 4);
|
|
if (int.TryParse(tagnostr, out int tagnoint))
|
|
{
|
|
var Changed = !old_CALString.Equals(tagnostr);
|
|
data.CallString = tagnostr;
|
|
data.CallNo = tagnoint;
|
|
old_CALString = tagnostr;
|
|
if (Changed) DataReceive?.Invoke(this, new DataEventArgs(DataType.CALL));
|
|
}
|
|
WriteData(MakeCheckSum("ACKCAL"));
|
|
}
|
|
else if (rcvdNow.StartsWith("CCA")) //능동형 Call에 의하여 들어 오는 구문
|
|
{
|
|
|
|
var tagnostr = encoding.GetString(bRcvData, 4, 4);
|
|
if (int.TryParse(tagnostr, out int tagnoint))
|
|
{
|
|
var Changed = !old_CCAString.Equals(tagnostr);
|
|
data.CCAString = tagnostr;
|
|
data.CCANo = tagnoint;
|
|
old_CCAString = tagnostr;
|
|
if (Changed) DataReceive?.Invoke(this, new DataEventArgs(DataType.CCA));
|
|
}
|
|
WriteData(MakeCheckSum("ACKCCA"));
|
|
}
|
|
}
|
|
#endregion
|
|
|
|
private string MakeCheckSum(string sCommand)
|
|
{
|
|
//sCommand = sCommand.ToUpper();
|
|
|
|
List<byte> buffer = new List<byte>();
|
|
buffer.Add(0x02);
|
|
|
|
//byte[] bCommand = System.Text.ASCIIEncoding.ASCII.GetBytes(sCommand);
|
|
buffer.AddRange(System.Text.ASCIIEncoding.ASCII.GetBytes(sCommand));
|
|
|
|
//make sum
|
|
int nSum = 0;
|
|
for (int nCnt = 1; nCnt < buffer.Count; nCnt++)
|
|
nSum += buffer[nCnt];
|
|
|
|
string sSum = nSum.ToString("X2").ToUpper();// Convert.ToString(nSum, 16).ToUpper();
|
|
if (sSum.Length < 2) sSum = "0" + sSum;
|
|
sSum = sSum.Substring(sSum.Length - 2);
|
|
|
|
//if (sSum.Length == 3)
|
|
// sSum = sSum.Remove(0, 1);
|
|
//else if (sSum.Length == 4)
|
|
// sSum = sSum.Remove(0, 2);
|
|
|
|
buffer.AddRange(System.Text.ASCIIEncoding.Default.GetBytes(sSum));
|
|
buffer.Add(0x03);
|
|
|
|
// byte[] bStxEtx = { 0x02, 0x03 };
|
|
|
|
|
|
|
|
//var orgstr = (System.Text.ASCIIEncoding.ASCII.GetString(bStxEtx, 0, 1) + sCommand + sSum + System.Text.ASCIIEncoding.ASCII.GetString(bStxEtx, 1, 1));
|
|
var newstr = System.Text.Encoding.Default.GetString(buffer.ToArray());
|
|
return newstr;
|
|
|
|
}
|
|
|
|
///// <summary>
|
|
///// commandQueue에 명령을 추가합니다(체크섬은 자동 추가됨)
|
|
///// </summary>
|
|
///// <param name="cmd">체크섬을 제외한 평문</param>
|
|
///// <param name="addlog">sendlog 에 추가할 경우 true를 입력하세요</param>
|
|
//public void AddCommand(string cmd, bool addlog = true, int Repeat = 1)
|
|
//{
|
|
// var fullcmd = MakeCheckSum(cmd);
|
|
// for (int i = 0; i < Repeat; i++)
|
|
// commandQueue.Enqueue(fullcmd);
|
|
// if (addlog)
|
|
// {
|
|
// Message?.Invoke(this, new MessageEventArgs()
|
|
// }
|
|
//}
|
|
|
|
public enum eStopOpt
|
|
{
|
|
Stop = 0,
|
|
MarkStop,
|
|
MarkRotateLeft,
|
|
MarkRotateRight,
|
|
MarkChangeDirection,
|
|
}
|
|
public enum eRunOpt
|
|
{
|
|
NotSet,
|
|
Forward,
|
|
Backward,
|
|
}
|
|
|
|
public enum ManulOpt
|
|
{
|
|
FS,
|
|
BS,
|
|
RT,
|
|
LT,
|
|
FL,
|
|
FR,
|
|
BL,
|
|
BR,
|
|
}
|
|
public enum Sensor
|
|
{
|
|
PBSOff = 0,
|
|
PBSOn,
|
|
AllOn,
|
|
}
|
|
public enum Speed
|
|
{
|
|
High,
|
|
Mid,
|
|
Low,
|
|
}
|
|
public enum LiftCommand
|
|
{
|
|
/// <summary>
|
|
/// lift up
|
|
/// </summary>
|
|
UP,
|
|
/// <summary>
|
|
/// lift down
|
|
/// </summary>
|
|
DN,
|
|
/// <summary>
|
|
/// lift 동작 정지
|
|
/// </summary>
|
|
STP,
|
|
/// <summary>
|
|
/// magnet holder on
|
|
/// </summary>
|
|
ON,
|
|
/// <summary>
|
|
/// magnet holder off
|
|
/// </summary>
|
|
OFF
|
|
}
|
|
|
|
|
|
private bool CheckSum(byte[] bData)
|
|
{
|
|
if (bData.Length < 2) // 데이터 길이가 2이하일 경우 비정상 처리
|
|
return false;
|
|
|
|
int nSum = 0;
|
|
// byte[] bData = encoding.GetBytes(sData);
|
|
|
|
for (int nCnt = 1; nCnt < bData.Length - 3; nCnt++)
|
|
{
|
|
nSum += bData[nCnt];
|
|
}
|
|
|
|
|
|
string sSum = Convert.ToString(nSum, 16).ToUpper();
|
|
|
|
string sCSTemp = string.Empty;
|
|
|
|
if (sSum.Length == 3)
|
|
sCSTemp = sSum.Remove(0, 1);
|
|
else if (sSum.Length == 4)
|
|
sCSTemp = sSum.Remove(0, 2);
|
|
|
|
if (bData[bData.Length - 2] == '*' && bData[bData.Length - 3] == '*')
|
|
return true;
|
|
|
|
if (sCSTemp[0] == (char)(bData[bData.Length - 3]) && sCSTemp[1] == (char)(bData[bData.Length - 2]))
|
|
return true;
|
|
else
|
|
return false;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
}
|