944 lines
32 KiB
C#
944 lines
32 KiB
C#
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using System.ComponentModel;
|
|
using System.Data;
|
|
using System.Drawing;
|
|
using System.Globalization;
|
|
using System.IO;
|
|
using System.Linq;
|
|
using System.Security.Cryptography.X509Certificates;
|
|
using System.Text;
|
|
using System.Windows.Forms;
|
|
using System.Windows.Forms.VisualStyles;
|
|
using static AGVEmulator.DevAGV;
|
|
using AGVNavigationCore.Controls;
|
|
using AGVNavigationCore.Models;
|
|
|
|
namespace AGVEmulator
|
|
{
|
|
public partial class fMain : Form
|
|
{
|
|
arUtil.Log logAGV, logBMS, logCAL;
|
|
DevBMS BMS;
|
|
DevAGV AGV;
|
|
DevXBE XBE;
|
|
|
|
// Map Control
|
|
private UnifiedAGVCanvas _agvCanvas;
|
|
private List<MapNode> _mapNodes;
|
|
private VirtualAGV _visualAgv;
|
|
|
|
// Emulator State
|
|
private MapNode _targetNode = null;
|
|
private bool _isTurning = false;
|
|
private double _targetAngle = 0;
|
|
private PointF _currentPosF; // For smooth movement
|
|
|
|
float BMS_MaxA = 80f;
|
|
float BMS_Remain = 79f;
|
|
float BMS_CurA = 70f;
|
|
float BMS_Volt = 25.4f;
|
|
|
|
UInt16[] cellvolt = new UInt16[] { 0, 0, 0, 0, 0, 0, 0, 0 };
|
|
|
|
|
|
public fMain()
|
|
{
|
|
InitializeComponent();
|
|
this.Text = $"{Application.ProductName} ver.{Application.ProductVersion}";
|
|
// logPLC = new arUtil.Log();
|
|
logAGV = new arUtil.Log();
|
|
logBMS = new arUtil.Log();
|
|
logCAL = new arUtil.Log();
|
|
|
|
// logPLC.FileNameFormat = "{yyyyMMdd}_PLC";
|
|
logAGV.FileNameFormat = "{yyyyMMdd}_AGV";
|
|
logBMS.FileNameFormat = "{yyyyMMdd}_BMS";
|
|
logCAL.FileNameFormat = "{yyyyMMdd}_CAL";
|
|
|
|
// logPLC.RaiseMsg += (s1, e1, d1) => { this.rtPLC.AddMsg(s1, e1, d1); };
|
|
logAGV.RaiseMsg += (s1, e1, d1) =>
|
|
{
|
|
if (d1.StartsWith("Tx"))
|
|
this.rtAGV.AddMsg(s1, e1, d1);
|
|
else
|
|
this.rtAGVPro.AddMsg(s1, e1, d1);
|
|
};
|
|
logBMS.RaiseMsg += (s1, e1, d1) => { this.rtBMS.AddMsg(s1, e1, d1); };
|
|
logCAL.RaiseMsg += (s1, e1, d1) => { this.rtCAL.AddMsg(s1, e1, d1); };
|
|
|
|
|
|
var logcolor = new arCtl.sLogMessageColor[] {
|
|
new arCtl.sLogMessageColor("NORM", Color.White),
|
|
new arCtl.sLogMessageColor("INF", Color.SkyBlue),
|
|
new arCtl.sLogMessageColor("WARN", Color.Tomato),
|
|
new arCtl.sLogMessageColor("ERR", Color.Red),
|
|
new arCtl.sLogMessageColor("RX", Color.Lime),
|
|
new arCtl.sLogMessageColor("TX", Color.Orange),
|
|
};
|
|
|
|
//rtPLC.ColorList = logcolor;
|
|
rtAGV.ColorList = logcolor;
|
|
rtBMS.ColorList = logcolor;
|
|
rtCAL.ColorList = logcolor;
|
|
rtAGVPro.ColorList = logcolor;
|
|
|
|
this.FormClosed += Form1_FormClosed;
|
|
|
|
BMS = new DevBMS();
|
|
AGV = new DevAGV();
|
|
XBE = new DevXBE();
|
|
this.serAGV.dev = AGV;
|
|
this.serBMS.dev = BMS;
|
|
this.serCAL.dev = XBE;
|
|
|
|
BMS.Message += BMS_Message;
|
|
AGV.Message += AGV_Message;
|
|
XBE.Message += CAL_Message;
|
|
|
|
BMS.RequestBatteryData += Bms_RequestBatteryData;
|
|
BMS.RequestVoltageData += BMS_RequestVoltageData;
|
|
XBE.ProtocReceived += CAL_ProtocReceived;
|
|
AGV.RequestStatusData += Agv_RequestStatusData;
|
|
AGV.ValueChanged += Agv_ValueChanged;
|
|
AGV.StsValueChanged += Agv_StsValueChanged;
|
|
AGV.Command += Agv_Command;
|
|
}
|
|
|
|
Random rnd;
|
|
private void Form1_Load(object sender, EventArgs e)
|
|
{
|
|
rnd = new Random(3000);
|
|
|
|
serAGV.BaudRate = 57600;
|
|
// serPLC.BaudRate = 57600;
|
|
trackBar1_Scroll(null, null);
|
|
trbT2_Scroll(null, null);
|
|
trbT1_Scroll(null, null);
|
|
timer1.Start();
|
|
|
|
//plc inout 이름 설정
|
|
List<string> titles = new List<string>();
|
|
List<bool> values = new List<bool>();
|
|
|
|
|
|
|
|
|
|
var arrs = Enum.GetNames(typeof(DevAGV.esystemflag0));
|
|
foreach (var item in arrs)
|
|
{
|
|
var data = (DevAGV.esystemflag0)Enum.Parse(typeof(DevAGV.esystemflag0), item);
|
|
|
|
var chk = new CheckBox();
|
|
chk.Text = $"[{(int)data:00}] {item}";
|
|
chk.AutoSize = true;
|
|
chk.Visible = true;
|
|
chk.Tag = (int)data;
|
|
chk.Dock = DockStyle.Top;
|
|
chk.CheckedChanged += Chk_CheckedChanged;
|
|
this.panel6.Controls.Add(chk);
|
|
chk.Checked = true; //기본값 rue
|
|
}
|
|
arrs = Enum.GetNames(typeof(DevAGV.esystemflag1));
|
|
foreach (var item in arrs)
|
|
{
|
|
var data = (DevAGV.esystemflag1)Enum.Parse(typeof(DevAGV.esystemflag1), item);
|
|
|
|
var chk = new CheckBox();
|
|
chk.Text = $"[{(int)data:00}] {item}";
|
|
chk.AutoSize = true;
|
|
chk.Visible = true;
|
|
chk.Tag = (int)data;
|
|
chk.Dock = DockStyle.Top;
|
|
chk.CheckedChanged += Chk_CheckedChanged;
|
|
this.panel7.Controls.Add(chk);
|
|
}
|
|
arrs = Enum.GetNames(typeof(DevAGV.esignal));
|
|
foreach (var item in arrs)
|
|
{
|
|
var data = (DevAGV.esignal)Enum.Parse(typeof(DevAGV.esignal), item);
|
|
|
|
var chk = new CheckBox();
|
|
chk.Text = $"[{(int)data:00}] {item}";
|
|
chk.AutoSize = true;
|
|
chk.Visible = true;
|
|
chk.Tag = (int)data;
|
|
chk.Dock = DockStyle.Top;
|
|
chk.CheckedChanged += Chk_CheckedChanged;
|
|
this.panel8.Controls.Add(chk);
|
|
}
|
|
arrs = Enum.GetNames(typeof(DevAGV.eerror));
|
|
foreach (var item in arrs)
|
|
{
|
|
var data = (DevAGV.eerror)Enum.Parse(typeof(DevAGV.eerror), item);
|
|
|
|
var chk = new CheckBox();
|
|
chk.Text = $"[{(int)data:00}] {item}";
|
|
chk.AutoSize = true;
|
|
chk.Visible = true;
|
|
chk.Tag = (int)data;
|
|
chk.Dock = DockStyle.Top;
|
|
chk.CheckedChanged += Chk_CheckedChanged;
|
|
this.panel9.Controls.Add(chk);
|
|
}
|
|
|
|
//기본 중지상태로 변환
|
|
AGV.SetAGV(DevAGV.esystemflag1.agv_stop, true);
|
|
|
|
foreach (RadioButton rad in groupBox5.Controls)
|
|
{
|
|
rad.CheckedChanged += (s1, e1) =>
|
|
{
|
|
if (rad.Checked)
|
|
{
|
|
AGV.sts_dir = rad.Text[0];
|
|
}
|
|
};
|
|
}
|
|
foreach (RadioButton rad in groupBox2.Controls)
|
|
{
|
|
rad.CheckedChanged += (s1, e1) =>
|
|
{
|
|
if (rad.Checked)
|
|
{
|
|
AGV.sts_bunki = rad.Text[0];
|
|
}
|
|
};
|
|
}
|
|
foreach (RadioButton rad in groupBox4.Controls)
|
|
{
|
|
rad.CheckedChanged += (s1, e1) =>
|
|
{
|
|
if (rad.Checked)
|
|
{
|
|
AGV.sts_speed = rad.Text[0];
|
|
}
|
|
};
|
|
}
|
|
foreach (RadioButton rad in groupBox8.Controls)
|
|
{
|
|
rad.CheckedChanged += (s1, e1) =>
|
|
{
|
|
if (rad.Checked)
|
|
{
|
|
AGV.sts_sensor = rad.Text[0];
|
|
}
|
|
};
|
|
}
|
|
|
|
MakeViewer();
|
|
InitializeMapControl();
|
|
|
|
|
|
agvViewer1.TagTouched += AgvViewer1_TagTouched;
|
|
agvViewer1.MarkTouched += AgvViewer1_MarkTouched;
|
|
agvViewer1.Command += AgvViewer1_Command;
|
|
}
|
|
|
|
void MakeViewer()
|
|
{
|
|
//태그목록
|
|
List<UC.AgvViewer.ptdata> listTAG = new List<UC.AgvViewer.ptdata>();
|
|
//var strtag = new string[] {
|
|
// "9000", "9001", //not
|
|
// "9010", "9011",
|
|
// //"9020", "9021",
|
|
// "9030", "9031",
|
|
// // "9040", "9041",
|
|
// "9050", "9051",
|
|
// // "9060", "9061",
|
|
// "9070", "9071",
|
|
// "9080", "9081",
|
|
// "9090", "9091"};
|
|
|
|
//foreach(var tag in strtag)
|
|
//{
|
|
// listTAG.Add(new UC.AgvViewer.ptdata {
|
|
// active=false,
|
|
// data = tag,
|
|
// pos =
|
|
// });
|
|
//}
|
|
//agvViewer1.listTAG = listTAG.ToArray();
|
|
|
|
//마크지점
|
|
List<UC.AgvViewer.ptdata> listMRK = new List<UC.AgvViewer.ptdata>();
|
|
var strmark = new string[] { "NOT", "QC", "CHG", "{0}", "#1", "{1}", "#2", "{2}", "#3", "{3}", "#4", "POT" };
|
|
var pos = 50;
|
|
var tagstart = 9000;
|
|
foreach (var item in strmark)
|
|
{
|
|
listMRK.Add(new UC.AgvViewer.ptdata
|
|
{
|
|
pos = pos,
|
|
data = item,
|
|
});
|
|
|
|
|
|
if (item.Equals("CHG"))
|
|
{
|
|
pos += 100;
|
|
continue;
|
|
}
|
|
else if (item.Equals("{0}")) tagstart = 9350;
|
|
else if (item.Equals("{1}")) tagstart = 9450;
|
|
else if (item.Equals("{2}")) tagstart = 9550;
|
|
else if (item.Equals("{3}")) tagstart = 9650;
|
|
else if (item.Equals("#1")) tagstart = 9400;
|
|
else if (item.Equals("#2")) tagstart = 9500;
|
|
else if (item.Equals("#3")) tagstart = 9600;
|
|
else if (item.Equals("#4")) tagstart = 9700;
|
|
|
|
|
|
//지정위치 좌우에 태그를 심는다
|
|
listTAG.Add(new UC.AgvViewer.ptdata
|
|
{
|
|
pos = pos - 20,
|
|
data = tagstart.ToString(),
|
|
});
|
|
|
|
if (item.Equals("NOT") == false && item.Equals("POT") == false && item.StartsWith("{") == false)
|
|
tagstart += 1;
|
|
|
|
|
|
listTAG.Add(new UC.AgvViewer.ptdata
|
|
{
|
|
pos = pos + 20,
|
|
data = tagstart.ToString(),
|
|
});
|
|
|
|
|
|
pos += 100;
|
|
|
|
if (item.Equals("NOT") == false && item.Equals("POT") == false && item.StartsWith("{") == false)
|
|
tagstart += 99;
|
|
else
|
|
tagstart += 100;
|
|
|
|
|
|
if (item.Equals("NOT"))
|
|
tagstart = 9300;
|
|
|
|
//else
|
|
//{
|
|
// pos += 100;
|
|
// tagstart += 10;
|
|
//}
|
|
|
|
}
|
|
agvViewer1.listMRK = listMRK.ToArray();
|
|
agvViewer1.listTAG = listTAG.ToArray();
|
|
agvViewer1.Invalidate();
|
|
|
|
}
|
|
private void AgvViewer1_Command(object sender, UC.AgvViewer.TagArgs e)
|
|
{
|
|
if (e.Data == "stop")
|
|
{
|
|
AGV.SetAGV(esystemflag1.agv_run, false);
|
|
AGV.SetAGV(esystemflag1.agv_stop, true);
|
|
logAGV.Add("시뮬로부터 자동 중지");
|
|
}
|
|
}
|
|
|
|
private void AgvViewer1_MarkTouched(object sender, UC.AgvViewer.TagArgs e)
|
|
{
|
|
// throw new NotImplementedException();
|
|
AGV.SetAGV(esignal.mark_sensor_1, e.Active);
|
|
logAGV.Add($"mark {e.Data} touch:{e.Active}");
|
|
}
|
|
|
|
private void AgvViewer1_TagTouched(object sender, UC.AgvViewer.TagArgs e)
|
|
{
|
|
logAGV.Add($"tag touch:{e.Data}");
|
|
numericUpDown1.Text = e.Data;// decimal.Parse(e.Data);
|
|
button18.PerformClick();
|
|
|
|
UpdateVisualAgvPosition(e.Data);
|
|
}
|
|
|
|
void InitializeMapControl()
|
|
{
|
|
_agvCanvas = new UnifiedAGVCanvas();
|
|
_agvCanvas.Dock = DockStyle.Fill;
|
|
_agvCanvas.Mode = UnifiedAGVCanvas.CanvasMode.Emulator; // Enable Emulator Mode
|
|
_agvCanvas.NodeRightClicked += _agvCanvas_NodeRightClicked;
|
|
this.tabPage4.Controls.Add(_agvCanvas);
|
|
|
|
// Load Map
|
|
// Try to find map file in standard location
|
|
string mapDir = Path.GetFullPath(Path.Combine(Application.StartupPath, @"..\..\..\..\Cs_HMI\Data"));
|
|
string mapPath = Path.Combine(mapDir, "NewMap.agvmap");
|
|
|
|
if (!File.Exists(mapPath))
|
|
{
|
|
if (Directory.Exists(mapDir))
|
|
{
|
|
var files = Directory.GetFiles(mapDir, "*.agvmap");
|
|
if (files.Length > 0) mapPath = files[0];
|
|
}
|
|
}
|
|
|
|
if (File.Exists(mapPath))
|
|
{
|
|
try
|
|
{
|
|
var mapresult = MapLoader.LoadMapFromFile(mapPath);
|
|
_mapNodes = mapresult.Nodes;
|
|
_agvCanvas.Nodes = _mapNodes;
|
|
_agvCanvas.FitToNodes();
|
|
|
|
// Initialize Visual AGV
|
|
if (_mapNodes.Count > 0)
|
|
{
|
|
_visualAgv = new VirtualAGV("AGV01", _mapNodes[0].Position);
|
|
_agvCanvas.AGVList = new List<IAGV> { _visualAgv };
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
logAGV.Add($"Map Load Error: {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
|
|
void UpdateVisualAgvPosition(string tag)
|
|
{
|
|
if (_visualAgv == null || _mapNodes == null) return;
|
|
|
|
// Find node by tag
|
|
// Assuming NodeId might be the tag or contain it
|
|
var node = _mapNodes.FirstOrDefault(n => n.NodeId == tag || n.NodeId.EndsWith(tag));
|
|
|
|
// If not found, try to parse tag as int and match
|
|
if (node == null && int.TryParse(tag, out int tagNum))
|
|
{
|
|
node = _mapNodes.FirstOrDefault(n => n.NodeId == tagNum.ToString());
|
|
}
|
|
|
|
if (node != null)
|
|
{
|
|
_visualAgv.SetPosition(node, (AGV.sts_dir == 'F' ? AgvDirection.Forward : AgvDirection.Backward));
|
|
UpdateVisualAGV();
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
char GetGroupItemCheckbox(GroupBox grp)
|
|
{
|
|
foreach (var ctl in grp.Controls)
|
|
{
|
|
if (ctl is RadioButton)
|
|
{
|
|
var rad = ctl as RadioButton;
|
|
if (rad.Checked) return rad.Text[0];
|
|
}
|
|
}
|
|
return '0';
|
|
}
|
|
|
|
|
|
|
|
|
|
int PLC_LeftPosition = 0;
|
|
int PLC_RightPosition = 0;
|
|
int PLC_LeftDir = 0;
|
|
int PLC_RightDir = 0;
|
|
|
|
|
|
public void SetBit(ref UInt16 _value, int idx, Boolean value)
|
|
{
|
|
if (value)
|
|
{
|
|
var offset = (UInt16)(1 << idx);
|
|
_value = (UInt16)(_value | offset);
|
|
}
|
|
else
|
|
{
|
|
var offset = (UInt16)(~(1 << idx));
|
|
_value = (UInt16)(_value & offset);
|
|
}
|
|
}
|
|
public void SetBit(ref byte _value, int idx, Boolean value)
|
|
{
|
|
if (value)
|
|
{
|
|
var offset = (byte)(1 << idx);
|
|
_value = (byte)(_value | offset);
|
|
}
|
|
else
|
|
{
|
|
var offset = (byte)(~(1 << idx));
|
|
_value = (byte)(_value & offset);
|
|
}
|
|
}
|
|
|
|
void aaplycheckboxbit(ref UInt16 v, Panel p)
|
|
{
|
|
foreach (CheckBox chk in p.Controls)
|
|
{
|
|
var idx = int.Parse(chk.Tag.ToString());
|
|
SetBit(ref v, idx, chk.Checked);
|
|
}
|
|
}
|
|
void aaplycheckboxbit(ref byte v, Panel p)
|
|
{
|
|
foreach (CheckBox chk in p.Controls)
|
|
{
|
|
var idx = int.Parse(chk.Tag.ToString());
|
|
SetBit(ref v, idx, chk.Checked);
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
private void button5_Click(object sender, EventArgs e)
|
|
{
|
|
AGV.WriteData("ACK");
|
|
}
|
|
|
|
private void button4_Click(object sender, EventArgs e)
|
|
{
|
|
AGV.WriteData("NAK");
|
|
}
|
|
|
|
|
|
|
|
private void trackBar1_Scroll(object sender, EventArgs e)
|
|
{
|
|
BMS_MaxA = float.Parse(label4.Text);
|
|
BMS_CurA = (float)(trackBar1.Value / 100f);
|
|
BMS_Remain = 100f * (BMS_CurA / BMS_MaxA);// (trackBar1.Value / 10f);
|
|
var minvolt = 20.2f;
|
|
var maxvolt = 26.8f;
|
|
// BMS_CurA = (BMS_MaxA * (BMS_Remain / 100f));
|
|
label3.Text = BMS_CurA.ToString("N0"); //curr amp
|
|
label5.Text = $"{BMS_Remain:N2}%";
|
|
BMS_Volt = minvolt + ((maxvolt - minvolt) * (BMS_CurA / BMS_MaxA));
|
|
label6.Text = $"{BMS_Volt:N2}v";
|
|
}
|
|
|
|
private void timer1_Tick(object sender, EventArgs e)
|
|
{
|
|
timer1.Stop();
|
|
sbAGV.ForeColor = AGV.IsOpen ? Color.ForestGreen : Color.Red;
|
|
// sbPLC.ForeColor = PLC.IsOpen ? Color.ForestGreen : Color.Red;
|
|
sbBMS.ForeColor = BMS.IsOpen ? Color.ForestGreen : Color.Red;
|
|
sbCAL.ForeColor = XBE.IsOpen ? Color.ForestGreen : Color.Red;
|
|
|
|
//if (checkBox2.Checked && PLC != null && PLC.IsOpen) PLC.AutoSendData(); //자동전송해야함
|
|
if (checkBox3.Checked && AGV != null && AGV.IsOpen) AGV.AutoSendData(); //자동전송해야함
|
|
if (BMS != null && BMS.IsOpen) BMS.AutoSendData(); //자동전송해야함
|
|
if (XBE != null && XBE.IsOpen) XBE.AutoSendData(); //자동전송해야함
|
|
|
|
if (chkSimulation.Checked && AGV.GetAGV(esystemflag1.agv_run))
|
|
{
|
|
if (radioButton15.Checked)
|
|
{
|
|
agvViewer1.dir = 1;
|
|
}
|
|
else
|
|
{
|
|
agvViewer1.dir = -1;
|
|
}
|
|
if (radioButton12.Checked) agvViewer1.mspd = 7;
|
|
else if (radioButton11.Checked) agvViewer1.mspd = 20;
|
|
else agvViewer1.mspd = 40;
|
|
agvViewer1.Invalidate();
|
|
|
|
UpdateEmulatorLogic();
|
|
}
|
|
|
|
|
|
timer1.Start();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
private void UpdateVisualAGV()
|
|
{
|
|
if (_visualAgv != null)
|
|
{
|
|
_agvCanvas.UpdateAGVPosition(_visualAgv.AgvId, _visualAgv.CurrentPosition);
|
|
// Update direction if needed, though UnifiedAGVCanvas uses Enum
|
|
// _agvCanvas.UpdateAGVDirection(_visualAgv.AgvId, _visualAgv.CurrentDirection);
|
|
_agvCanvas.Invalidate();
|
|
}
|
|
}
|
|
|
|
private void UpdateEmulatorLogic()
|
|
{
|
|
if (_visualAgv == null || _mapNodes == null) return;
|
|
|
|
// Initialize float position if needed
|
|
if (_currentPosF.IsEmpty) _currentPosF = _visualAgv.CurrentPosition;
|
|
|
|
// Movement Logic
|
|
double speed = (agvViewer1.mspd > 0 ? agvViewer1.mspd : 10) * 0.2; // Scale speed
|
|
|
|
if (_isTurning)
|
|
{
|
|
// Turn Logic: 90 deg in 5 sec = 18 deg/sec
|
|
// Timer is likely 100ms? (Default WinForms timer) -> 1.8 deg/tick
|
|
double turnSpeed = 1.8;
|
|
|
|
double diff = _targetAngle - _visualAgv.Angle;
|
|
// Normalize diff to -180 to 180
|
|
while (diff > 180) diff -= 360;
|
|
while (diff <= -180) diff += 360;
|
|
|
|
if (Math.Abs(diff) < turnSpeed)
|
|
{
|
|
_visualAgv.Angle = _targetAngle;
|
|
_isTurning = false;
|
|
}
|
|
else
|
|
{
|
|
_visualAgv.Angle += Math.Sign(diff) * turnSpeed;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Move Forward
|
|
double rad = _visualAgv.Angle * Math.PI / 180.0;
|
|
_currentPosF.X += (float)(speed * Math.Cos(rad));
|
|
_currentPosF.Y += (float)(speed * Math.Sin(rad));
|
|
|
|
_visualAgv.CurrentPosition = Point.Round(_currentPosF);
|
|
|
|
// Check for Nodes (RFID Trigger)
|
|
foreach (var node in _mapNodes)
|
|
{
|
|
double dist = Math.Sqrt(Math.Pow(node.Position.X - _visualAgv.CurrentPosition.X, 2) + Math.Pow(node.Position.Y - _visualAgv.CurrentPosition.Y, 2));
|
|
if (dist < 15) // Hit Node
|
|
{
|
|
// Send Tag
|
|
if (node.NodeId != numericUpDown1.Text)
|
|
{
|
|
if (int.TryParse(node.NodeId, out int tag))
|
|
{
|
|
AGV.SendTag(node.NodeId);
|
|
numericUpDown1.Text = node.NodeId;
|
|
|
|
// Snap to node
|
|
_currentPosF = node.Position;
|
|
_visualAgv.CurrentPosition = node.Position;
|
|
|
|
// Decide Next Move (Turn/Straight)
|
|
DecideNextMove(node);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
UpdateVisualAGV();
|
|
}
|
|
|
|
private void DecideNextMove(MapNode currentNode)
|
|
{
|
|
// Simple logic:
|
|
// If AGV.sts_bunki == 'L', turn -90
|
|
// If AGV.sts_bunki == 'R', turn +90
|
|
// Else keep straight
|
|
|
|
if (AGV.sts_bunki == 'L')
|
|
{
|
|
_targetAngle = _visualAgv.Angle - 90;
|
|
_isTurning = true;
|
|
}
|
|
else if (AGV.sts_bunki == 'R')
|
|
{
|
|
_targetAngle = _visualAgv.Angle + 90;
|
|
_isTurning = true;
|
|
}
|
|
|
|
// Normalize Target Angle
|
|
while (_targetAngle >= 360) _targetAngle -= 360;
|
|
while (_targetAngle < 0) _targetAngle += 360;
|
|
}
|
|
|
|
private void _agvCanvas_NodeRightClicked(object sender, MapNode e)
|
|
{
|
|
if (e != null && _visualAgv != null)
|
|
{
|
|
_visualAgv.CurrentPosition = e.Position;
|
|
_currentPosF = e.Position;
|
|
|
|
if (int.TryParse(e.NodeId, out int tag))
|
|
{
|
|
numericUpDown1.Text = tag.ToString();
|
|
}
|
|
UpdateVisualAGV();
|
|
|
|
|
|
private void button14_Click(object sender, EventArgs e)
|
|
{
|
|
//agv 정지
|
|
//비상정지 에러 플래그
|
|
AGV.SetAGV(DevAGV.eerror.Emergency, true);
|
|
AGV.SetAGV(DevAGV.esystemflag1.agv_run, false);
|
|
AGV.SetAGV(DevAGV.esystemflag1.agv_stop, true);
|
|
}
|
|
|
|
|
|
|
|
private void button16_Click(object sender, EventArgs e)
|
|
{
|
|
AGV.SetAGV(DevAGV.esystemflag1.stop_by_front_detect, true);
|
|
}
|
|
|
|
private void button15_Click(object sender, EventArgs e)
|
|
{
|
|
AGV.SetAGV(DevAGV.esystemflag1.stop_by_front_detect, false);
|
|
}
|
|
|
|
private void button17_Click(object sender, EventArgs e)
|
|
{
|
|
//agv 정지
|
|
//비상정지 에러 플래그
|
|
AGV.SetAGV(DevAGV.eerror.Emergency, false);
|
|
AGV.SetAGV(DevAGV.eerror.line_out_error, false);
|
|
AGV.SetAGV(DevAGV.eerror.Overcurrent, false);
|
|
AGV.SetAGV(DevAGV.esystemflag1.agv_run, false);
|
|
AGV.SetAGV(DevAGV.esystemflag1.agv_stop, true);
|
|
}
|
|
|
|
|
|
|
|
private void Chk_CheckedChanged(object sender, EventArgs e)
|
|
{
|
|
var chk = sender as CheckBox;
|
|
var idx = int.Parse(chk.Tag.ToString());
|
|
|
|
switch (chk.Parent.Tag.ToString().ToLower())
|
|
{
|
|
case "s0":
|
|
var v0 = (DevAGV.esystemflag0)idx;
|
|
AGV.SetAGV(v0, chk.Checked);
|
|
break;
|
|
case "s1":
|
|
var v1 = (DevAGV.esystemflag1)idx;
|
|
AGV.SetAGV(v1, chk.Checked);
|
|
break;
|
|
case "er":
|
|
var v2 = (DevAGV.eerror)idx;
|
|
AGV.SetAGV(v2, chk.Checked);
|
|
break;
|
|
case "sg":
|
|
var v3 = (DevAGV.esignal)idx;
|
|
AGV.SetAGV(v3, chk.Checked);
|
|
break;
|
|
}
|
|
|
|
chk.BackColor = chk.Checked ? Color.Lime : SystemColors.Window;
|
|
}
|
|
|
|
private void button18_Click(object sender, EventArgs e)
|
|
{
|
|
if (int.TryParse(numericUpDown1.Text, out int tagno))
|
|
{
|
|
AGV.SendTag(tagno.ToString());
|
|
numericUpDown1.SelectAll();
|
|
numericUpDown1.Focus();
|
|
}
|
|
}
|
|
|
|
private void agvViewer1_MouseDown(object sender, MouseEventArgs e)
|
|
{
|
|
if (e.Button == MouseButtons.Left)
|
|
{
|
|
agvViewer1.StopbyMark = !agvViewer1.StopbyMark;
|
|
}
|
|
else if (e.Button == MouseButtons.Right)
|
|
{
|
|
MakeViewer();
|
|
}
|
|
}
|
|
|
|
private void button6_Click(object sender, EventArgs e)
|
|
{
|
|
var target = (byte)nudIDAgv.Value;
|
|
var tagno = (uint)nudTagNo.Value;
|
|
this.XBE.SendGotoTag(target, tagno);
|
|
}
|
|
|
|
private void button1_Click(object sender, EventArgs e)
|
|
{
|
|
var target = (byte)nudIDAgv.Value;
|
|
var tagno = (uint)numericUpDown2.Value;
|
|
this.XBE.SendCurrentPos(target, tagno);
|
|
}
|
|
|
|
UInt16 Temp1, Temp2;
|
|
private void trbT1_Scroll(object sender, EventArgs e)
|
|
{
|
|
//값에 /10해서 표시한다.
|
|
Temp1 = (UInt16)trbT1.Value;
|
|
label10.Text = $"{Temp1 / 10f}º";
|
|
|
|
}
|
|
|
|
private void numericUpDown1_KeyDown(object sender, KeyEventArgs e)
|
|
{
|
|
if (e.KeyCode == Keys.Enter)
|
|
button18.PerformClick();
|
|
}
|
|
|
|
private void toolStripButton3_Click(object sender, EventArgs e)
|
|
{
|
|
var file = @"C:\Data\Amkor\AGV4\route\NewMap.agvmap";
|
|
LoadMapFile(file);
|
|
}
|
|
private string _currentMapFilePath;
|
|
private void LoadMapFile(string filePath)
|
|
{
|
|
try
|
|
{
|
|
var result = MapLoader.LoadMapFromFile(filePath);
|
|
|
|
if (result.Success)
|
|
{
|
|
Console.WriteLine($"Map File Load : {filePath}");
|
|
|
|
_mapNodes = result.Nodes;
|
|
_currentMapFilePath = filePath;
|
|
|
|
// RFID 자동 할당 제거 - 에디터에서 설정한 값 그대로 사용
|
|
|
|
// 시뮬레이터 캔버스에 맵 설정
|
|
this._agvCanvas.Nodes = _mapNodes;
|
|
|
|
// 맵 설정 적용 (배경색, 그리드 표시)
|
|
if (result.Settings != null)
|
|
{
|
|
_agvCanvas.BackColor = System.Drawing.Color.FromArgb(result.Settings.BackgroundColorArgb);
|
|
_agvCanvas.ShowGrid = result.Settings.ShowGrid;
|
|
}
|
|
|
|
// 맵에 맞춤
|
|
_agvCanvas.FitToNodes();
|
|
}
|
|
else
|
|
{
|
|
throw new InvalidOperationException($"맵 파일 로드 실패: {result.ErrorMessage}");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
throw new InvalidOperationException($"맵 파일 로드 실패: {ex.Message}", ex);
|
|
}
|
|
}
|
|
|
|
private void toolStripButton4_Click(object sender, EventArgs e)
|
|
{
|
|
// Change rotation (180 degrees)
|
|
if (_visualAgv != null)
|
|
{
|
|
_visualAgv.Angle = (_visualAgv.Angle + 180) % 360;
|
|
UpdateVisualAGV();
|
|
}
|
|
}
|
|
|
|
private void toolStripButton5_Click(object sender, EventArgs e)
|
|
{
|
|
// Set AGV Position by RFID
|
|
string input = "";
|
|
using (Form form = new Form())
|
|
{
|
|
form.Text = "Set AGV Position";
|
|
form.Size = new Size(300, 150);
|
|
Label lbl = new Label() { Left = 20, Top = 20, Text = "Enter RFID (Node ID):", AutoSize = true };
|
|
TextBox txt = new TextBox() { Left = 20, Top = 45, Width = 240 };
|
|
Button btn = new Button() { Text = "OK", Left = 180, Width = 80, Top = 75, DialogResult = DialogResult.OK };
|
|
form.Controls.Add(lbl);
|
|
form.Controls.Add(txt);
|
|
form.Controls.Add(btn);
|
|
form.AcceptButton = btn;
|
|
form.StartPosition = FormStartPosition.CenterParent;
|
|
|
|
if (form.ShowDialog() == DialogResult.OK)
|
|
{
|
|
input = txt.Text;
|
|
}
|
|
}
|
|
|
|
if (!string.IsNullOrEmpty(input) && _mapNodes != null && _visualAgv != null)
|
|
{
|
|
var node = _mapNodes.FirstOrDefault(n => n.NodeId == input);
|
|
if (node != null)
|
|
{
|
|
_visualAgv.CurrentPosition = node.Position;
|
|
_currentPosF = node.Position;
|
|
numericUpDown1.Text = node.NodeId;
|
|
|
|
// Auto-orient: Prefer Right (0) or Up (270)
|
|
var neighbors = _mapNodes.Where(n => n != node &&
|
|
Math.Sqrt(Math.Pow(n.Position.X - node.Position.X, 2) + Math.Pow(n.Position.Y - node.Position.Y, 2)) < 150)
|
|
.ToList();
|
|
|
|
if (neighbors.Any())
|
|
{
|
|
var rightNeighbor = neighbors.FirstOrDefault(n => n.Position.X > node.Position.X && Math.Abs(n.Position.Y - node.Position.Y) < 50);
|
|
if (rightNeighbor != null)
|
|
{
|
|
_visualAgv.Angle = 0;
|
|
}
|
|
else
|
|
{
|
|
var upNeighbor = neighbors.FirstOrDefault(n => n.Position.Y < node.Position.Y && Math.Abs(n.Position.X - node.Position.X) < 50);
|
|
if (upNeighbor != null)
|
|
{
|
|
_visualAgv.Angle = 270;
|
|
}
|
|
else
|
|
{
|
|
var first = neighbors.First();
|
|
double angle = Math.Atan2(first.Position.Y - node.Position.Y, first.Position.X - node.Position.X) * 180 / Math.PI;
|
|
if (angle < 0) angle += 360;
|
|
_visualAgv.Angle = angle;
|
|
}
|
|
}
|
|
}
|
|
|
|
UpdateVisualAGV();
|
|
}
|
|
else
|
|
{
|
|
MessageBox.Show($"Node '{input}' not found.", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
|
|
}
|
|
}
|
|
}
|
|
private void trbT2_Scroll(object sender, EventArgs e)
|
|
{
|
|
Temp2 = (UInt16)trbT2.Value;
|
|
label11.Text = $"{Temp2 / 10f}º";
|
|
}
|
|
|
|
private void toolStripButton1_Click(object sender, EventArgs e)
|
|
{
|
|
serAGV.Connect();
|
|
serBMS.Connect();
|
|
serCAL.Connect();
|
|
}
|
|
|
|
private void toolStripButton2_Click(object sender, EventArgs e)
|
|
{
|
|
serAGV.Disconnect();
|
|
serBMS.Disconnect();
|
|
serCAL.Disconnect();
|
|
}
|
|
|
|
|
|
}
|
|
}
|
|
|
|
|