Amkor/ENIG
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ing...

Browse Source
This commit is contained in:
backuppc
2025-12-12 17:27:50 +09:00
parent 07ddc0425f
commit 4e9d29d22f
44 changed files with 7173 additions and 61 deletions
Download Patch File Download Diff File Expand all files Collapse all files

587
AGVEmulator/DevAGV.cs Normal file
View File

@@ -0,0 +1,587 @@
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 eCommand : byte
{
LOAD = 0, //EEPROM 불러오기
SAVE, //EEPROM 저장
RESET, //초기화
PINGCHK,
SET_PINMODE, //PINMODE 설정
SET_DOUTPUT, //디지털출력설정(포트번호,값[1,0])
SET_AOUTPUT, //아날로그출력설정(포트GET_SETTING = 50, //셋팅값 요청
SET_FLAG,
SET_EEPROM,
SET_MANUALSPEED,
GET_SETTING = 50,
GUIDE_MOT = 90, //가이드커버(양쪽) 0=멈춤,1=UP,2=DN 아스키코드표 90=Z
SET_EEP_DIREV,
};
public enum DOName
{
PINO_GUIDEMOTOR_LDIR,
PINO_GUIDEMOTOR_LRUN,
PINO_GUIDEMOTOR_RDIR,
PINO_GUIDEMOTOR_RRUN,
PINO_EMPTY_26,
PINO_EMPTY_27,
PINO_EMPTY_28,
PINO_EMPTY_29,
}
public enum DIName
{
PINI_EMG,
PINI_BTN_1,
PINI_BTN_2,
PINI_BTN_3,
PINI_BTN_4,
PINI_OVERLOADL,
PINI_OVERLOADR,
PINI_EMPTY_36,
PINI_EMPTY_37,
PINI_EMPTY_38,
PINI_BTN_ZUP,
PINI_BTN_ZDN,
PINI_LIMIT_LU,
PINI_LIMIT_LD,
PINI_LIMIT_RU,
PINI_LIMIT_RD,
PINI_STOP,
}
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;
}
}
}