249 lines
12 KiB
C#
249 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Data;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using arCtl;
|
|
using Project.StateMachine;
|
|
using COMM;
|
|
using AR;
|
|
using AGVNavigationCore.Models;
|
|
using AGVNavigationCore.Controls;
|
|
|
|
namespace Project
|
|
{
|
|
|
|
public partial class fMain
|
|
{
|
|
private void AGV_Message(object sender, arDev.Narumi.MessageEventArgs e)
|
|
{
|
|
if (e.MsgType == arDev.arRS232.MessageType.Normal)
|
|
|
|
PUB.logagv.AddE(e.Message);
|
|
else if (e.MsgType == arDev.arRS232.MessageType.Normal)
|
|
|
|
PUB.logagv.Add(e.Message);
|
|
else if (e.MsgType == arDev.arRS232.MessageType.Recv)
|
|
{
|
|
if (e.Message.Substring(1).StartsWith("STS") == false)
|
|
PUB.logagv.Add("AGV-RX", e.Message);
|
|
}
|
|
else if (e.MsgType == arDev.arRS232.MessageType.Send)
|
|
PUB.logagv.Add("AGV-TX", e.Message);
|
|
else
|
|
{
|
|
|
|
PUB.logagv.Add(e.MsgType.ToString(), e.Message);
|
|
}
|
|
}
|
|
|
|
|
|
bool _charging = false;
|
|
private void AGV_DataReceive(object sender, arDev.Narumi.DataEventArgs e)
|
|
{
|
|
try
|
|
{
|
|
//데이터 파싱
|
|
switch (e.DataType)
|
|
{
|
|
case arDev.Narumi.DataType.STS:
|
|
{
|
|
//마크센서 확인
|
|
var agv_err = PUB.AGV.error.Value;
|
|
var agv_emg = PUB.AGV.error.Emergency;
|
|
var agv_chg = PUB.AGV.system1.Battery_charging;
|
|
var agv_stp = PUB.AGV.system1.agv_stop;
|
|
var agv_run = PUB.AGV.system1.agv_run;
|
|
var agv_mrk = PUB.AGV.signal.mark_sensor;
|
|
|
|
|
|
//if (chg_run && PUB.AGV.system1.agv_run) PUB.Speak("이동을 시작 합니다");
|
|
VAR.BOOL[eVarBool.AGVDIR_BACK] = PUB.AGV.data.Direction == 'B';
|
|
var syncDir = PUB.AGV.data.Direction == 'B' ? AgvDirection.Backward : AgvDirection.Forward;
|
|
|
|
// [Sync] Update VirtualAGV Direction
|
|
if (PUB._virtualAGV != null)
|
|
{
|
|
if (PUB._virtualAGV.CurrentDirection != syncDir)
|
|
PUB.UpdateAGVDirection(syncDir);
|
|
}
|
|
|
|
|
|
// [Sync] Update VirtualAGV State
|
|
AGVState syncState = AGVState.Idle;
|
|
if (PUB.AGV.error.Value > 0) syncState = AGVState.Error;
|
|
else if (PUB.AGV.system1.Battery_charging) syncState = AGVState.Charging;
|
|
else if (PUB.AGV.system1.agv_run) syncState = AGVState.Moving;
|
|
|
|
if (PUB._virtualAGV != null && PUB._virtualAGV.GetCurrentState() != syncState)
|
|
PUB.UpdateAGVState(syncState);
|
|
|
|
if (VAR.BOOL[eVarBool.AGV_ERROR] != (agv_err > 0))
|
|
{
|
|
VAR.BOOL[eVarBool.AGV_ERROR] = (agv_err > 0);
|
|
PUB.logagv.Add($"new AGV_ERROR ({PUB.AGV.error.Value})");
|
|
}
|
|
if (VAR.BOOL[eVarBool.EMERGENCY] != agv_emg)
|
|
{
|
|
VAR.BOOL[eVarBool.EMERGENCY] = agv_emg;
|
|
PUB.logagv.Add($"new EMERGENCY ({VAR.BOOL[eVarBool.EMERGENCY]})");
|
|
}
|
|
|
|
|
|
//차징상태변경
|
|
if (_charging != agv_chg)
|
|
{
|
|
if (agv_chg)
|
|
{
|
|
VAR.TIME[eVarTime.ChargeStart] = DateTime.Now;
|
|
PUB.logagv.Add($"충전시작:{VAR.TIME[eVarTime.ChargeStart]}");
|
|
}
|
|
_charging = agv_chg;
|
|
}
|
|
|
|
//배터리충전상태
|
|
if (VAR.BOOL[eVarBool.FLAG_CHARGEONA] != agv_chg)
|
|
{
|
|
PUB.log.Add($"충전상태전환 {agv_chg}");
|
|
VAR.BOOL[eVarBool.FLAG_CHARGEONA] = agv_chg;
|
|
}
|
|
//자동충전해제시 곧바로 수동 충전되는 경우가 있어 자동 상태를 BMS에 넣는다 230118
|
|
PUB.BMS.AutoCharge = agv_chg;
|
|
|
|
if (PUB.AGV.error.Charger_pos_error != VAR.BOOL[eVarBool.CHG_POSERR])
|
|
{
|
|
if (PUB.AGV.error.Charger_pos_error)
|
|
{
|
|
PUB.Speak(Lang.충전기위치오류);
|
|
}
|
|
VAR.BOOL[eVarBool.CHG_POSERR] = PUB.AGV.error.Charger_pos_error;
|
|
}
|
|
|
|
//나르미가 멈췄다면 다음 마크 이동 기능이 OFF 된다
|
|
if (agv_stp)
|
|
{
|
|
if (VAR.BOOL[eVarBool.NEXTSTOP_MARK])
|
|
{
|
|
VAR.BOOL[eVarBool.NEXTSTOP_MARK] = false;
|
|
PUB.logagv.Add($"NEXTSTOP_MARK 변경({VAR.BOOL[eVarBool.NEXTSTOP_MARK]})");
|
|
}
|
|
}
|
|
|
|
//마크센서 상태가 변경이 되었다면
|
|
if (VAR.BOOL[eVarBool.MARK_SENSOR] != PUB.AGV.signal.mark_sensor)
|
|
{
|
|
PUB.logagv.Add($"MARK_SENSOR 변경({PUB.AGV.signal.mark_sensor})");
|
|
VAR.BOOL[eVarBool.MARK_SENSOR] = PUB.AGV.signal.mark_sensor;
|
|
|
|
//AGV가 멈췄고 마크센서가 ON되었다면 마지막 RFID 위치가 확정된경우이다
|
|
if (agv_stp && VAR.BOOL[eVarBool.MARK_SENSOR])
|
|
{
|
|
PUB.log.Add("마크스탑이 확인되어 최종위치를 PASS 처리 합니다");
|
|
var curnode = PUB._virtualAGV.SetCurrentNodeMarkStop();
|
|
if (curnode == true)
|
|
{
|
|
PUB.log.Add($"마크스탑으로 해당노드의 이동을 확정합니다");
|
|
}
|
|
else PUB.log.AddAT($"마크스탑이 확인되었으나 현재 노드가없어 PASS를 설정하지 못함");
|
|
}
|
|
}
|
|
if (VAR.BOOL[eVarBool.MARK_SENSOROFF] != VAR.BOOL[eVarBool.MARK_SENSOR])
|
|
{
|
|
VAR.BOOL[eVarBool.MARK_SENSOROFF] = !VAR.BOOL[eVarBool.MARK_SENSOR];
|
|
VAR.TIME[eVarTime.MarkSensorOff] = DateTime.Now;
|
|
PUB.log.Add($"MARK_SENSOROFF 변경({VAR.BOOL[eVarBool.MARK_SENSOROFF]})");
|
|
}
|
|
|
|
}
|
|
break;
|
|
case arDev.Narumi.DataType.TAG:
|
|
{
|
|
//자동 실행 중이다.
|
|
|
|
PUB.Result.LastTAG = PUB.AGV.data.TagNo.ToString("0000");
|
|
PUB.log.Add($"AGV 태그수신 : {PUB.AGV.data.TagNo} LastTag:{PUB.Result.LastTAG}");
|
|
//POT/NOT 보면 일단 바로 멈추게한다
|
|
if (PUB.Result.CurrentPos == ePosition.POT || PUB.Result.CurrentPos == ePosition.NOT)
|
|
{
|
|
var logEMsg = $"Stop by [POT/NOT]";
|
|
PUB.AGV.AGVMoveStop(logEMsg);
|
|
PUB.log.AddE(logEMsg);
|
|
}
|
|
|
|
//virtual agv setting
|
|
var CurrentNode = PUB._mapCanvas.Nodes.FirstOrDefault(t => t.RfidId.Equals(PUB.Result.LastTAG, StringComparison.OrdinalIgnoreCase));
|
|
if (CurrentNode == null)
|
|
{
|
|
//없는 노드는 자동으로 추가한다
|
|
var newNodeId = $"AUTO_{PUB.Result.LastTAG}";
|
|
var newNode = new MapNode
|
|
{
|
|
Id = newNodeId,
|
|
RfidId = PUB.Result.LastTAG,
|
|
Position = new Point(100, 100), // 기본 위치
|
|
IsActive = true,
|
|
CreatedDate = DateTime.Now,
|
|
ModifiedDate = DateTime.Now
|
|
};
|
|
|
|
// 맵 노드 리스트에 추가
|
|
PUB._mapCanvas.Nodes.Add(newNode);
|
|
|
|
// 캔버스에 노드 반영 (재설정)
|
|
PUB._mapCanvas.Nodes = PUB._mapCanvas.Nodes;
|
|
|
|
// 로그 기록
|
|
PUB.log.AddI($"RFID:{PUB.Result.LastTAG} 노드를 자동 추가했습니다 (NodeId: {newNodeId})");
|
|
|
|
// CurrentNode에 새로 생성한 노드 할당
|
|
CurrentNode = newNode;
|
|
}
|
|
else
|
|
{
|
|
//모터방향 확인해서 UI와 AGV클래스에 적용한다
|
|
var MotDireciton = PUB.AGV.data.Direction == 'B' ? AGVNavigationCore.Models.AgvDirection.Backward : AGVNavigationCore.Models.AgvDirection.Forward;
|
|
PUB._mapCanvas.SetAGVPosition(PUB.setting.MCID, CurrentNode, MotDireciton);
|
|
PUB._virtualAGV.SetPosition(CurrentNode, MotDireciton);
|
|
}
|
|
|
|
//태그를 읽었다면 상태를 바로 전송한다
|
|
PUB.XBE.SendStatus();
|
|
}
|
|
break;
|
|
case arDev.Narumi.DataType.ACK:
|
|
PUB.logagv.Add($"AGV_[ACK]Receive : {PUB.AGV.ACKData}");
|
|
break;
|
|
default:
|
|
PUB.logagv.Add($"AGV_DataReceive : {e.DataType}");
|
|
break;
|
|
|
|
}
|
|
|
|
//이 후 상황을 예측한다
|
|
if (PUB._mapCanvas != null && PUB._virtualAGV != null)
|
|
{
|
|
var nextAction = PUB._virtualAGV.Predict();
|
|
var message = $"[다음 행동 예측]\n\n" +
|
|
$"모터: {nextAction.Motor}\n" +
|
|
$"마그넷: {nextAction.Magnet}\n" +
|
|
$"속도: {nextAction.Speed}\n" +
|
|
$"이유: {nextAction.Message}\n\n" +
|
|
$"---\n" +
|
|
$"현재 상태: {PUB._virtualAGV.CurrentState}\n" +
|
|
$"현재 방향: {PUB._virtualAGV.CurrentDirection}\n" +
|
|
$"위치 확정: {PUB._virtualAGV.IsPositionConfirmed} (RFID {PUB._virtualAGV.DetectedRfidCount}개)\n" +
|
|
$"현재 노드: {PUB._virtualAGV.CurrentNode.Id ?? "없음"}";
|
|
|
|
PUB._mapCanvas.PredictMessage = message;
|
|
}
|
|
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Console.WriteLine($"[AGV_DataReceive] {ex.Message}");
|
|
}
|
|
}
|
|
}
|
|
}
|