538 lines
18 KiB
C#
538 lines
18 KiB
C#
using arCtl;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Drawing;
|
||
using System.Linq;
|
||
|
||
namespace AGVEmulator
|
||
{
|
||
class DevAGV : AR.Dev.RS232
|
||
{
|
||
//######### private variable
|
||
byte runtime = 0;
|
||
|
||
//######### public variable
|
||
public UInt16 system0 = 0;
|
||
public UInt16 system1 = 0;
|
||
public UInt16 error = 0;
|
||
public byte signal = 0;
|
||
public char sts_bunki = 'S';
|
||
public char sts_speed = 'L';
|
||
public char sts_dir = 'F';
|
||
public char sts_sensor = '1';
|
||
|
||
public enum eerror
|
||
{
|
||
Emergency = 0,
|
||
Overcurrent,
|
||
Charger_run_error,
|
||
Charger_pos_error,
|
||
line_out_error = 4,
|
||
|
||
/// <summary>
|
||
/// 기동시 자석 감지 에러
|
||
/// </summary>
|
||
runerror_by_no_magent_line,
|
||
/// <summary>
|
||
/// 호출제어기 통신 오류
|
||
/// </summary>
|
||
controller_comm_error = 11,
|
||
/// <summary>
|
||
/// 도착경보기 통신 오류
|
||
/// </summary>
|
||
arrive_ctl_comm_error,
|
||
|
||
/// <summary>
|
||
/// 자동문제어기 통신 오류
|
||
/// </summary>
|
||
door_ctl_comm_error,
|
||
|
||
/// <summary>
|
||
/// 자동충전기 통신 오류
|
||
/// </summary>
|
||
charger_comm_error,
|
||
|
||
/// <summary>
|
||
/// 교차로 제어기 통신 오류
|
||
/// </summary>
|
||
cross_ctrl_comm_error,
|
||
}
|
||
public enum esignal
|
||
{
|
||
front_gate_out = 0,
|
||
rear_sensor_out,
|
||
mark_sensor_1,
|
||
mark_sensor_2,
|
||
front_left_sensor,
|
||
front_right_sensor,
|
||
front_center_sensor,
|
||
charger_align_sensor,
|
||
}
|
||
public enum esystemflag0
|
||
{
|
||
Memory_RW_State = 5,
|
||
EXT_IO_Conn_State,
|
||
RFID_Conn_State,
|
||
M5E_Module_Run_State = 8,
|
||
Front_Ultrasonic_Conn_State,
|
||
Front_Untrasonic_Sensor_State,
|
||
Side_Ultrasonic_Conn_State,
|
||
Side_Ultrasonic_Sensor_State = 12,
|
||
Front_Guide_Sensor_State,
|
||
Rear_Guide_Sensor_State,
|
||
Battery_Level_Check
|
||
}
|
||
public enum esystemflag1
|
||
{
|
||
Side_Detect_Ignore = 3,
|
||
Melody_check,
|
||
Mark2_check,
|
||
Mark1_check,
|
||
gateout_check,
|
||
Battery_charging = 8,
|
||
re_Start,
|
||
|
||
/// <summary>
|
||
/// 전방 감지 무시
|
||
/// </summary>
|
||
front_detect_ignore,
|
||
|
||
/// <summary>
|
||
/// 전방장애물감지상태
|
||
/// </summary>
|
||
front_detect_check,
|
||
|
||
/// <summary>
|
||
/// 전방감지 후 정지 상태
|
||
/// </summary>
|
||
stop_by_front_detect = 12,
|
||
/// <summary>
|
||
/// 교차로 진입 후 정지 상태
|
||
/// </summary>
|
||
stop_by_cross_in,
|
||
agv_stop,
|
||
agv_run
|
||
}
|
||
|
||
public enum evaluetype
|
||
{
|
||
system0,
|
||
system1,
|
||
error,
|
||
signal,
|
||
}
|
||
public enum estsvaluetype
|
||
{
|
||
bunki,
|
||
speed,
|
||
direction,
|
||
sensor
|
||
}
|
||
public class ValueChangedArgs : EventArgs
|
||
{
|
||
public int Idx { get; set; }
|
||
public bool Value { get; set; }
|
||
public evaluetype vtype { get; set; }
|
||
public ValueChangedArgs(int idx, bool val, evaluetype isOut)
|
||
{
|
||
this.Idx = idx;
|
||
this.Value = val;
|
||
this.vtype = isOut;
|
||
}
|
||
}
|
||
public class StsValueChangedArgs : EventArgs
|
||
{
|
||
public char Value { get; set; }
|
||
public estsvaluetype vtype { get; set; }
|
||
public StsValueChangedArgs(estsvaluetype vType, char val)
|
||
{
|
||
this.Value = val;
|
||
this.vtype = vType;
|
||
}
|
||
}
|
||
|
||
public class RequestStatusDataArgs : EventArgs
|
||
{
|
||
public UInt16 system0 { get; set; } = 0;
|
||
public UInt16 system1 { get; set; } = 0;
|
||
public char speed { get; set; } = 'L';
|
||
public char direction { get; set; } = 'F';
|
||
public char bunki { get; set; } = 'S';
|
||
|
||
public byte signal { get; set; } = 0;
|
||
public float volt { get; set; } = 0f;
|
||
public UInt16 error { get; set; } = 0;
|
||
public char sensor { get; set; } = '0';
|
||
public RequestStatusDataArgs()
|
||
{
|
||
|
||
}
|
||
}
|
||
public event EventHandler<RequestStatusDataArgs> RequestStatusData;
|
||
public event EventHandler<ValueChangedArgs> ValueChanged;
|
||
public event EventHandler<StsValueChangedArgs> StsValueChanged;
|
||
|
||
public class commandargs : EventArgs
|
||
{
|
||
public string Command { get; set; }
|
||
public commandargs(string cmd)
|
||
{
|
||
this.Command = cmd;
|
||
}
|
||
}
|
||
public event EventHandler<commandargs> Command;
|
||
public class FrameData
|
||
{
|
||
public string Checksum { get; set; }
|
||
public byte stx { get; set; }
|
||
public byte etx { get; set; }
|
||
public string cmd { get; set; }
|
||
public string data { get; set; }
|
||
public FrameData(byte[] data)
|
||
{
|
||
this.stx = data[0];
|
||
this.etx = data[data.Length - 1];
|
||
this.Checksum = System.Text.Encoding.Default.GetString(data, data.Length - 3, 2);
|
||
this.data = System.Text.Encoding.Default.GetString(data, 1, data.Length - 4);
|
||
this.cmd = this.data.Substring(0, 3);
|
||
this.data = this.data.Substring(3);
|
||
}
|
||
}
|
||
|
||
|
||
#region SetAGV/SetSTS/SetBIT/GetBIT
|
||
Boolean GetBit(ref UInt16 _value, int idx)
|
||
{
|
||
var offset = (UInt16)(1 << idx);
|
||
return (_value & offset) != 0;
|
||
}
|
||
|
||
Boolean GetBit(ref byte _value, int idx)
|
||
{
|
||
var offset = (byte)(1 << idx);
|
||
return (_value & offset) != 0;
|
||
}
|
||
|
||
bool SetBit(ref UInt16 _value, int idx, Boolean value)
|
||
{
|
||
var oldvalue = GetBit(ref _value, idx);
|
||
if (value)
|
||
{
|
||
var offset = (UInt16)(1 << idx);
|
||
_value = (UInt16)(_value | offset);
|
||
}
|
||
else
|
||
{
|
||
var offset = (UInt16)(~(1 << idx));
|
||
_value = (UInt16)(_value & offset);
|
||
}
|
||
return oldvalue != value;
|
||
}
|
||
bool SetBit(ref byte _value, int idx, Boolean value)
|
||
{
|
||
var oldvalue = GetBit(ref _value, idx);
|
||
if (value)
|
||
{
|
||
var offset = (byte)(1 << idx);
|
||
_value = (byte)(_value | offset);
|
||
}
|
||
else
|
||
{
|
||
var offset = (byte)(~(1 << idx));
|
||
_value = (byte)(_value & offset);
|
||
}
|
||
return oldvalue != value;
|
||
}
|
||
public bool GetAGV(DevAGV.esystemflag1 flag)
|
||
{
|
||
var idx = (int)flag;
|
||
return GetBit(ref system1, idx);
|
||
}
|
||
public void SetAGV(DevAGV.esystemflag0 flag, bool value)
|
||
{
|
||
var idx = (int)flag;
|
||
if (SetBit(ref system0, idx, value))
|
||
ValueChanged?.Invoke(this, new ValueChangedArgs(idx, value, evaluetype.system0));
|
||
}
|
||
public void SetAGV(DevAGV.esystemflag1 flag, bool value)
|
||
{
|
||
var idx = (int)flag;
|
||
if (SetBit(ref system1, idx, value))
|
||
ValueChanged?.Invoke(this, new ValueChangedArgs(idx, value, evaluetype.system1));
|
||
}
|
||
public void SetAGV(DevAGV.eerror flag, bool value)
|
||
{
|
||
var idx = (int)flag;
|
||
if (SetBit(ref error, idx, value))
|
||
ValueChanged?.Invoke(this, new ValueChangedArgs(idx, value, evaluetype.error));
|
||
}
|
||
public void SetAGV(DevAGV.esignal flag, bool value)
|
||
{
|
||
var idx = (int)flag;
|
||
if (SetBit(ref signal, idx, value))
|
||
ValueChanged?.Invoke(this, new ValueChangedArgs(idx, value, evaluetype.signal));
|
||
}
|
||
|
||
public void SetSTS(estsvaluetype target, char value)
|
||
{
|
||
//bool changed = false;
|
||
switch (target)
|
||
{
|
||
case estsvaluetype.sensor:
|
||
//changed = this.sts_sensor.Equals(value);
|
||
sts_sensor = value;
|
||
break;
|
||
case estsvaluetype.direction:
|
||
//changed = this.sts_dir.Equals(value);
|
||
sts_dir = value;
|
||
break;
|
||
case estsvaluetype.speed:
|
||
//changed = this.sts_speed.Equals(value);
|
||
sts_speed = value;
|
||
break;
|
||
case estsvaluetype.bunki:
|
||
//changed = this.sts_bunki.Equals(value);
|
||
sts_bunki = value;
|
||
break;
|
||
}
|
||
//if(changed)
|
||
{
|
||
StsValueChanged?.Invoke(this, new StsValueChangedArgs(target, value));
|
||
}
|
||
}
|
||
#endregion
|
||
|
||
public override bool ProcessRecvData(byte[] data)
|
||
{
|
||
//dd로 시작하고 34개의 데이터
|
||
//STS258FFFF40000000LSF0000003A
|
||
//02 53 54 53 32 35 38 46 46 46 46 34 30 30 30 30 30 30 30 4C 53 46 30 30 30 30 30 30 33 41 03
|
||
var frame = new FrameData(data);
|
||
|
||
switch (frame.cmd)
|
||
{
|
||
case "CRN": //기동명령
|
||
//sts_dir = frame.data[0];
|
||
SetSTS(estsvaluetype.direction, frame.data[0]);
|
||
SetAGV(esystemflag1.agv_stop, false);
|
||
SetAGV(esystemflag1.agv_run, true);
|
||
break;
|
||
case "CST": //중지명령
|
||
//(바로 중지한다)
|
||
if (frame.data.StartsWith("M"))
|
||
{
|
||
Command?.Invoke(this, new commandargs("stopmark"));
|
||
}
|
||
else
|
||
{
|
||
SetAGV(esystemflag1.agv_run, false);
|
||
SetAGV(esystemflag1.agv_stop, true);
|
||
}
|
||
|
||
break;
|
||
case "CBR": //분기명령
|
||
//FSL
|
||
SetSTS(estsvaluetype.direction, frame.data[0]);
|
||
SetSTS(estsvaluetype.bunki, frame.data[1]);
|
||
SetSTS(estsvaluetype.speed, frame.data[2]);
|
||
SetSTS(estsvaluetype.sensor, frame.data[3]);
|
||
break;
|
||
case "CRT"://수동제어
|
||
sts_dir = frame.data[0];
|
||
sts_bunki = frame.data[1];
|
||
sts_speed = frame.data[2];
|
||
sts_sensor = frame.data[3];
|
||
SetAGV(esystemflag1.agv_stop, false);
|
||
SetAGV(esystemflag1.agv_run, true);
|
||
break;
|
||
case "ACK": //응답ok
|
||
RaiseMessage(MessageType.Normal, $">> {frame.cmd} DATA={frame.data}");
|
||
break;
|
||
case "SFR": //reset
|
||
SetAGV(DevAGV.eerror.Emergency, false);
|
||
SetAGV(DevAGV.eerror.line_out_error, false);
|
||
SetAGV(DevAGV.eerror.Overcurrent, false);
|
||
SetAGV(DevAGV.esystemflag1.agv_run, false);
|
||
SetAGV(DevAGV.esystemflag1.agv_stop, true);
|
||
break;
|
||
case "CBT"://충전작업
|
||
SetAGV(DevAGV.esystemflag1.Battery_charging, true);
|
||
var id = frame.data.Substring(2, 2);
|
||
var cmd = frame.data[4];
|
||
var delaytime = int.Parse(frame.data.Substring(5));
|
||
if (cmd == 'I') SetAGV(DevAGV.esystemflag1.Battery_charging, true);
|
||
else SetAGV(esystemflag1.Battery_charging, false);
|
||
break;
|
||
case "SSH":
|
||
case "SSM":
|
||
case "SSL":
|
||
case "SSS":
|
||
case "SHS":
|
||
case "SLS":
|
||
case "SPK":
|
||
case "SPM":
|
||
case "SPL":
|
||
case "SPS":
|
||
case "SIK":
|
||
case "SIM":
|
||
case "SIH":
|
||
case "SIL":
|
||
case "SIS":
|
||
case "SDK":
|
||
case "SDW":
|
||
case "SDL":
|
||
case "SDS":
|
||
case "SRS":
|
||
case "SCK":
|
||
case "SSK":
|
||
case "STT":
|
||
case "SSI":
|
||
case "SMD":
|
||
case "SSC":
|
||
case "SPN":
|
||
case "SPH":
|
||
case "SCH":
|
||
case "SDH":
|
||
case "SDM":
|
||
case "SLB":
|
||
case "SGS":
|
||
SendCmd("ACK", frame.cmd);
|
||
break;
|
||
default:
|
||
RaiseMessage(MessageType.Normal, $"미처리명령 {frame.cmd} DATA={frame.data}");
|
||
break;
|
||
}
|
||
|
||
|
||
|
||
return true;
|
||
}
|
||
public void SendCmd(string cmd, string value)
|
||
{
|
||
|
||
var barr = new List<byte>();
|
||
barr.Add(0x02);
|
||
barr.AddRange(System.Text.Encoding.Default.GetBytes(cmd));
|
||
barr.AddRange(System.Text.Encoding.Default.GetBytes(value));
|
||
barr.Add((byte)'*');
|
||
barr.Add((byte)'*');
|
||
barr.Add(0x03);
|
||
var cmdstr = System.Text.Encoding.Default.GetString(barr.ToArray());
|
||
RaiseMessage(MessageType.Normal, "Tx:" + barr.ToArray().HexString());
|
||
WriteData(barr.ToArray());
|
||
}
|
||
|
||
public void SendTag(string tagno)
|
||
{
|
||
tagno = tagno.PadLeft(6, '0');
|
||
if (tagno.Length > 6) tagno = tagno.Substring(0, 6);
|
||
var barr = new List<byte>();
|
||
barr.Add(0x02);
|
||
barr.Add((byte)'T');
|
||
barr.Add((byte)'A');
|
||
barr.Add((byte)'G');
|
||
barr.AddRange(System.Text.Encoding.Default.GetBytes(tagno));
|
||
barr.Add((byte)'*');
|
||
barr.Add((byte)'*');
|
||
barr.Add(0x03);
|
||
var cmdstr = System.Text.Encoding.Default.GetString(barr.ToArray());
|
||
RaiseMessage(MessageType.Normal, "Tx:" + barr.ToArray().HexString());
|
||
WriteData(barr.ToArray());
|
||
}
|
||
|
||
public override void AutoSendData()
|
||
{
|
||
//if (_device.DtrEnable == false) return;
|
||
//STS258FFFF40000000LSF0000003A
|
||
//02 53 54 53 32 35 38 46 46 46 46 34 30 30 30 30 30 30 30 4C 53 46 30 30 30 30 30 30 33 41 03
|
||
var sample = "02 53 54 53 32 35 38 46 46 46 46 34 30 30 30 30 30 30 30 4C 53 46 30 30 30 30 30 30 33 41 03";
|
||
var barr = sample.Split(' ').ToList().Select(t => Convert.ToByte(t, 16)).ToArray();
|
||
|
||
//if (RequestStatusData != null)
|
||
//{
|
||
//var p = new RequestStatusDataArgs();
|
||
//RequestStatusData.Invoke(this, p);
|
||
|
||
var voltstr = "255";// ((int)(p.volt * 10f)).ToString().PadRight(3, '0');
|
||
var bufarr = System.Text.Encoding.Default.GetBytes(voltstr);
|
||
Array.Copy(bufarr, 0, barr, 4, bufarr.Length);
|
||
|
||
bufarr = System.Text.Encoding.Default.GetBytes(system0.ToString("X2").PadLeft(4, '0'));
|
||
|
||
Array.Copy(bufarr, 0, barr, 7, bufarr.Length);
|
||
|
||
bufarr = System.Text.Encoding.Default.GetBytes(system1.ToString("X2").PadLeft(4, '0'));
|
||
Array.Copy(bufarr, 0, barr, 11, bufarr.Length);
|
||
|
||
bufarr = System.Text.Encoding.Default.GetBytes(error.ToString("X2").PadLeft(4, '0'));
|
||
Array.Copy(bufarr, 0, barr, 15, bufarr.Length);
|
||
|
||
|
||
barr[19] = (byte)this.sts_speed;
|
||
barr[20] = (byte)this.sts_bunki;
|
||
barr[21] = (byte)this.sts_dir;
|
||
barr[22] = (byte)this.sts_sensor;
|
||
//bufarr = System.Text.Encoding.Default.GetBytes(p.sensor.ToString().PadLeft(2, '0'));
|
||
//Array.Copy(bufarr, 0, barr, 22, bufarr.Length);
|
||
bufarr = System.Text.Encoding.Default.GetBytes(signal.ToString("X2").PadLeft(2, '0'));
|
||
Array.Copy(bufarr, 0, barr, 23, bufarr.Length);
|
||
//barr[22] = (byte)'5';
|
||
|
||
barr[barr.Length - 3] = (byte)'*';
|
||
barr[barr.Length - 2] = (byte)'*';
|
||
|
||
//}
|
||
|
||
var cmdstr = System.Text.Encoding.Default.GetString(barr);
|
||
RaiseMessage(MessageType.Normal, "Tx:" + barr.ToArray().HexString());
|
||
WriteData(barr.ToArray());
|
||
}
|
||
|
||
|
||
protected override bool CustomParser(byte[] buf, out byte[] remainBuffer)
|
||
{
|
||
//DD A5 03 00 FF FD 77 0D
|
||
//remainBuffer = new byte[] { };
|
||
List<byte> remain = new List<byte>();
|
||
bool retval = false;
|
||
foreach (var b in buf)
|
||
{
|
||
if (retval)
|
||
{
|
||
remain.Add(b);
|
||
continue;
|
||
}
|
||
|
||
|
||
|
||
if (b == 0x02) //stx
|
||
{
|
||
tempBuffer.Clear();
|
||
tempBuffer.Add(b);
|
||
}
|
||
|
||
else if (b == 0x03) //etx
|
||
{
|
||
tempBuffer.Add(b);
|
||
retval = true;
|
||
}
|
||
else
|
||
{
|
||
//데이터길이가 만족한 상태
|
||
tempBuffer.Add(b);
|
||
var maxlen = 100;
|
||
if (tempBuffer.Count > maxlen)
|
||
{
|
||
RaiseMessage(MessageType.Error, $"buffer over({maxlen})");
|
||
tempBuffer.Clear();
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
remainBuffer = remain.ToArray();
|
||
return retval;
|
||
}
|
||
}
|
||
}
|