945 lines
31 KiB
C#
945 lines
31 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using AGVNavigationCore.Controls;
|
|
using AGVNavigationCore.Models;
|
|
using AGVNavigationCore.PathFinding;
|
|
using AGVNavigationCore.PathFinding.Core;
|
|
|
|
namespace AGVNavigationCore.Models
|
|
{
|
|
/// <summary>
|
|
/// 가상 AGV 클래스 (코어 비즈니스 로직)
|
|
/// 실제 AGV와 시뮬레이터 모두에서 사용 가능한 공용 로직
|
|
/// 시뮬레이션과 실제 동작이 동일하게 동작하도록 설계됨
|
|
/// </summary>
|
|
public class VirtualAGV : IMovableAGV, IAGV
|
|
{
|
|
#region Events
|
|
|
|
/// <summary>
|
|
/// AGV 상태 변경 이벤트
|
|
/// </summary>
|
|
public event EventHandler<AGVState> StateChanged;
|
|
|
|
/// <summary>
|
|
/// 위치 변경 이벤트
|
|
/// </summary>
|
|
public event EventHandler<(Point, AgvDirection, MapNode)> PositionChanged;
|
|
|
|
/// <summary>
|
|
/// RFID 감지 이벤트
|
|
/// </summary>
|
|
public event EventHandler<string> RfidDetected;
|
|
|
|
/// <summary>
|
|
/// 경로 완료 이벤트
|
|
/// </summary>
|
|
public event EventHandler<AGVPathResult> PathCompleted;
|
|
|
|
/// <summary>
|
|
/// 오류 발생 이벤트
|
|
/// </summary>
|
|
public event EventHandler<string> ErrorOccurred;
|
|
|
|
|
|
|
|
#endregion
|
|
|
|
#region Fields
|
|
|
|
private string _agvId;
|
|
private Point _currentPosition;
|
|
private Point _prevPosition;
|
|
private AgvDirection _currentDirection;
|
|
private AgvDirection _prevDirection;
|
|
private AGVState _currentState;
|
|
private float _currentSpeed;
|
|
|
|
// 경로 관련
|
|
private AGVPathResult _currentPath;
|
|
private List<string> _remainingNodes;
|
|
private int _currentNodeIndex;
|
|
private MapNode _currentNode;
|
|
private MapNode _prevNode;
|
|
private AGVTurn _turn;
|
|
|
|
// 이동 관련
|
|
private DateTime _lastUpdateTime;
|
|
private Point _moveStartPosition;
|
|
private Point _moveTargetPosition;
|
|
|
|
// 도킹 관련
|
|
private DockingDirection _dockingDirection;
|
|
|
|
// 시뮬레이션 설정
|
|
private readonly float _moveSpeed = 50.0f; // 픽셀/초
|
|
private bool _isMoving;
|
|
|
|
// RFID 위치 추적 (실제 AGV용)
|
|
private List<string> _detectedRfids = new List<string>(); // 감지된 RFID 목록
|
|
private bool _isPositionConfirmed = false; // 위치 확정 여부 (RFID 2개 이상 감지)
|
|
|
|
// 에뮬레이터용 추가 속성
|
|
public double Angle { get; set; } = 0; // 0 = Right, 90 = Down, 180 = Left, 270 = Up (Standard Math)
|
|
// But AGV Direction: Forward usually means "Front of AGV".
|
|
// Let's assume Angle is the orientation of the AGV in degrees.
|
|
|
|
public bool IsStopMarkOn { get; set; } = false;
|
|
|
|
#endregion
|
|
|
|
#region Properties
|
|
|
|
|
|
/// <summary>
|
|
/// 대상 이동시 모터 방향
|
|
/// </summary>
|
|
public AgvDirection PrevDirection => _prevDirection;
|
|
|
|
|
|
/// <summary>
|
|
/// AGV ID
|
|
/// </summary>
|
|
public string AgvId => _agvId;
|
|
|
|
/// <summary>
|
|
/// 현재 위치
|
|
/// </summary>
|
|
public Point CurrentPosition
|
|
{
|
|
get => _currentPosition;
|
|
set => _currentPosition = value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 방향
|
|
/// 모터의 동작 방향
|
|
/// </summary>
|
|
public AgvDirection CurrentDirection
|
|
{
|
|
get => _currentDirection;
|
|
set => _currentDirection = value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 상태
|
|
/// </summary>
|
|
public AGVState CurrentState
|
|
{
|
|
get => _currentState;
|
|
set => _currentState = value;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 속도
|
|
/// </summary>
|
|
public float CurrentSpeed => _currentSpeed;
|
|
|
|
/// <summary>
|
|
/// 현재 경로
|
|
/// </summary>
|
|
public AGVPathResult CurrentPath => _currentPath;
|
|
|
|
public void ClearPath()
|
|
{
|
|
_currentPath = null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 노드 ID
|
|
/// </summary>
|
|
public MapNode CurrentNode => _currentNode;
|
|
|
|
/// <summary>
|
|
/// 현재 노드 ID (CurrentNode.Id)
|
|
/// </summary>
|
|
public string CurrentNodeId => _currentNode?.Id;
|
|
|
|
/// <summary>
|
|
/// 현재노드의 RFID(id)값을 표시합니다 없는경우 (X)가 표시됩니다
|
|
/// </summary>
|
|
public string CurrentNodeID2
|
|
{
|
|
get
|
|
{
|
|
if (_currentNode == null) return "(X)";
|
|
return _currentNode.ID2;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이전 위치
|
|
/// </summary>
|
|
public Point? PrevPosition => _prevPosition;
|
|
|
|
/// <summary>
|
|
/// 배터리 레벨 (시뮬레이션)
|
|
/// </summary>
|
|
public float BatteryLevel { get; set; } = 100.0f;
|
|
|
|
|
|
/// <summary>
|
|
/// 이전 노드
|
|
/// </summary>
|
|
public MapNode PrevNode => _prevNode;
|
|
|
|
/// <summary>
|
|
/// Turn 상태값
|
|
/// </summary>
|
|
public AGVTurn Turn { get; set; }
|
|
|
|
/// <summary>
|
|
/// 도킹 방향
|
|
/// </summary>
|
|
public DockingDirection DockingDirection => _dockingDirection;
|
|
|
|
/// <summary>
|
|
/// 위치 확정 여부 (RFID 2개 이상 감지 시 true)
|
|
/// </summary>
|
|
public bool IsPositionConfirmed => _isPositionConfirmed;
|
|
|
|
/// <summary>
|
|
/// 감지된 RFID 개수
|
|
/// </summary>
|
|
public int DetectedRfidCount => _detectedRfids.Count;
|
|
|
|
/// <summary>
|
|
/// 배터리 부족 경고 임계값 (%)
|
|
/// </summary>
|
|
public float LowBatteryThreshold { get; set; } = 20.0f;
|
|
|
|
#endregion
|
|
|
|
#region Constructor
|
|
|
|
/// <summary>
|
|
/// 생성자
|
|
/// </summary>
|
|
/// <param name="agvId">AGV ID</param>
|
|
/// <param name="startPosition">시작 위치</param>
|
|
/// <param name="startDirection">시작 방향</param>
|
|
public VirtualAGV(string agvId, Point startPosition, AgvDirection startDirection = AgvDirection.Forward)
|
|
{
|
|
_agvId = agvId;
|
|
_currentPosition = startPosition;
|
|
_currentDirection = startDirection;
|
|
_currentState = AGVState.Idle;
|
|
_currentSpeed = 0;
|
|
_dockingDirection = DockingDirection.Forward; // 기본값: 전진 도킹
|
|
_currentNode = null;
|
|
_prevNode = null;
|
|
_isMoving = false;
|
|
_lastUpdateTime = DateTime.Now;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - 센서/RFID 입력 (실제 AGV에서 호출)
|
|
|
|
/// <summary>
|
|
/// 현재 위치 설정 (실제 AGV 센서에서)
|
|
/// </summary>
|
|
public void SetCurrentPosition(Point position)
|
|
{
|
|
_currentPosition = position;
|
|
}
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// 감지된 RFID 설정 (실제 RFID 센서에서)
|
|
/// </summary>
|
|
public void SetDetectedRfid(string rfidId)
|
|
{
|
|
// RFID 목록에 추가 (중복 제거)
|
|
if (!_detectedRfids.Contains(rfidId))
|
|
{
|
|
_detectedRfids.Add(rfidId);
|
|
}
|
|
|
|
// RFID 2개 이상 감지 시 위치 확정
|
|
if (_detectedRfids.Count >= 2 && !_isPositionConfirmed)
|
|
{
|
|
_isPositionConfirmed = true;
|
|
}
|
|
|
|
RfidDetected?.Invoke(this, rfidId);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 모터 방향 설정 (실제 모터 컨트롤러에서)
|
|
/// </summary>
|
|
public void SetMotorDirection(AgvDirection direction)
|
|
{
|
|
_currentDirection = direction;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 배터리 레벨 설정 (실제 BMS에서)
|
|
/// </summary>
|
|
public void SetBatteryLevel(float percentage)
|
|
{
|
|
BatteryLevel = Math.Max(0, Math.Min(100, percentage));
|
|
|
|
// 배터리 부족 경고
|
|
if (BatteryLevel < LowBatteryThreshold && _currentState != AGVState.Charging)
|
|
{
|
|
OnError($"배터리 부족: {BatteryLevel:F1}% (기준: {LowBatteryThreshold}%)");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 현재 노드id의 개체를 IsPass 로 설정합니다
|
|
/// </summary>
|
|
public bool SetCurrentNodeMarkStop()
|
|
{
|
|
if (_currentNode == null) return false;
|
|
if (_currentPath == null) return false;
|
|
var 미완료된처음노드 = _currentPath.DetailedPath.Where(t => t.IsPass == false).OrderBy(t => t.seq).FirstOrDefault();
|
|
if (미완료된처음노드 == null) return false;
|
|
미완료된처음노드.IsPass = true;
|
|
Console.WriteLine($"미완료된처음노드를 true러치합니다");
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 다음 동작 예측 (실제 AGV 제어용)
|
|
/// AGV가 지속적으로 호출하여 현재 상태와 예측 상태를 일치시킴
|
|
/// </summary>
|
|
/// <returns>다음에 수행할 모터/마그넷/속도 명령</returns>
|
|
public AGVCommand Predict()
|
|
{
|
|
|
|
// 1. 위치 미확정 상태 (RFID 2개 미만 감지)
|
|
if (!_isPositionConfirmed)
|
|
{
|
|
// 항상 전진 + 저속으로 이동 (RFID 감지 대기)
|
|
return new AGVCommand(
|
|
MotorCommand.Forward,
|
|
MagnetPosition.S, // 직진
|
|
SpeedLevel.L, // 저속
|
|
eAGVCommandReason.UnknownPosition,
|
|
$"위치 미확정 (RFID {_detectedRfids.Count}/2) - 전진하여 RFID 탐색"
|
|
);
|
|
}
|
|
|
|
// 2. 위치 확정됨 + 경로 없음 → 정지 (목적지 미설정 상태)
|
|
if (_currentPath == null || (_currentPath.DetailedPath?.Count ?? 0) < 1)
|
|
{
|
|
var curpos = "알수없음";
|
|
if (_currentNode != null)
|
|
{
|
|
curpos = _currentNode.HasRfid() ? $"RFID #{_currentNode.RfidId} (*{_currentNode.Id})" : $"(*{_currentNode.Id})";
|
|
}
|
|
return new AGVCommand(
|
|
MotorCommand.Stop,
|
|
MagnetPosition.S,
|
|
SpeedLevel.L,
|
|
eAGVCommandReason.NoPath,
|
|
$"(목적지 미설정) - 현재={curpos}"
|
|
);
|
|
}
|
|
|
|
// 3. 위치 확정됨 + 경로 있음 + 남은 노드 없음 → 정지 (목적지 도착)
|
|
var lastNode = _currentPath.DetailedPath.Last();
|
|
if (_currentPath.DetailedPath.Where(t => t.seq < lastNode.seq && t.IsPass == false).Any() == false)
|
|
{
|
|
// 마지막 노드에 도착했는지 확인 (현재 노드가 마지막 노드와 같은지) -
|
|
// 모터방향오 같아야한다. 간혹 방향전환 후 MARK STOP하는경우가있다. 260127
|
|
if (_currentNode != null && _currentNode.Id == lastNode.NodeId && lastNode.MotorDirection == CurrentDirection)
|
|
{
|
|
|
|
if (lastNode.IsPass) //이미완료되었다.
|
|
{
|
|
return new AGVCommand(
|
|
MotorCommand.Stop,
|
|
MagnetPosition.S,
|
|
SpeedLevel.L,
|
|
eAGVCommandReason.Complete,
|
|
$"목적지 도착 - 최종:{CurrentNodeID2}"
|
|
);
|
|
}
|
|
else
|
|
{
|
|
//움직이지 않는다면 움직이게하고, 움직인다면 마크스탑하낟.i
|
|
|
|
|
|
//도킹노드라면 markstop 을 나머지는 바로 스탑한다.
|
|
eAGVCommandReason reason = eAGVCommandReason.MarkStop;
|
|
if (_targetnode.StationType == StationType.Normal || _targetnode.StationType == StationType.Limit)
|
|
{
|
|
//일반노드는 마크스탑포인트가 없으니 바로 종료되도록 한다
|
|
reason = eAGVCommandReason.Complete;
|
|
}
|
|
|
|
//마지막노드는 일혔지만 완료되지 않았다. 마크스탑필요
|
|
return new AGVCommand(
|
|
MotorCommand.Stop,
|
|
MagnetPosition.S,
|
|
SpeedLevel.L,
|
|
reason,
|
|
$"목적지 도착 전(MarkStop) - 최종:{CurrentNodeID2}"
|
|
);
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
// 4. 경로이탈
|
|
var TargetNode = _currentPath.DetailedPath.Where(t => t.IsPass == false && t.NodeId.Equals(_currentNode.Id)).FirstOrDefault();
|
|
if (TargetNode == null)
|
|
{
|
|
return new AGVCommand(
|
|
MotorCommand.Stop,
|
|
MagnetPosition.S,
|
|
SpeedLevel.L,
|
|
eAGVCommandReason.PathOut,
|
|
$"(재탐색요청)경로이탈 현재위치:{CurrentNodeID2}"
|
|
);
|
|
}
|
|
|
|
return GetCommandFromPath(CurrentNode, "경로 실행 시작");
|
|
|
|
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - 상태 조회
|
|
|
|
/// <summary>
|
|
/// 현재 위치 조회
|
|
/// </summary>
|
|
public Point GetCurrentPosition() => _currentPosition;
|
|
|
|
/// <summary>
|
|
/// 현재 상태 조회
|
|
/// </summary>
|
|
public AGVState GetCurrentState() => _currentState;
|
|
|
|
/// <summary>
|
|
/// 현재 노드 ID 조회
|
|
/// </summary>
|
|
public MapNode GetCurrentNode() => _currentNode;
|
|
|
|
/// <summary>
|
|
/// AGV 정보 조회
|
|
/// </summary>
|
|
public string GetStatus()
|
|
{
|
|
return $"AGV[{_agvId}] 위치:({_currentPosition.X},{_currentPosition.Y}) " +
|
|
$"방향:{_currentDirection} 상태:{_currentState} " +
|
|
$"속도:{_currentSpeed:F1} 배터리:{BatteryLevel:F1}%";
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - 경로 실행
|
|
|
|
/// <summary>
|
|
/// 경로가 설정되어있는지?
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public bool HasPath()
|
|
{
|
|
if (_currentPath == null) return false;
|
|
if (_currentPath.DetailedPath == null) return false;
|
|
return _currentPath.DetailedPath.Any();
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로 설정 (실제 AGV 및 시뮬레이터에서 사용)
|
|
/// </summary>
|
|
/// <param name="path">실행할 경로</param>
|
|
public void SetPath(AGVPathResult path)
|
|
{
|
|
if (path == null)
|
|
{
|
|
_currentPath = null;
|
|
_remainingNodes.Clear();// = null;
|
|
_currentNodeIndex = 0;
|
|
OnError("경로가 null입니다.");
|
|
return;
|
|
}
|
|
|
|
_currentPath = path;
|
|
_remainingNodes = path.Path.Select(n => n.Id).ToList(); // MapNode → NodeId 변환
|
|
_currentNodeIndex = 0;
|
|
|
|
// 경로 시작 노드가 현재 노드와 다른 경우 경고
|
|
if (_currentNode != null && _remainingNodes.Count > 0 && _remainingNodes[0] != _currentNode.Id)
|
|
{
|
|
OnError($"경로 시작 노드({_remainingNodes[0]})와 현재 노드({_currentNode.Id})가 다릅니다.");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 경로 정지
|
|
/// </summary>
|
|
public void StopPath()
|
|
{
|
|
_isMoving = false;
|
|
_currentPath = null;
|
|
_remainingNodes?.Clear();
|
|
SetState(AGVState.Idle);
|
|
_currentSpeed = 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 긴급 정지
|
|
/// </summary>
|
|
public void EmergencyStop()
|
|
{
|
|
StopPath();
|
|
OnError("긴급 정지가 실행되었습니다.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// 일시 정지 (경로 유지)
|
|
/// </summary>
|
|
public void Pause()
|
|
{
|
|
_isMoving = false;
|
|
_currentSpeed = 0;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 이동 재개
|
|
/// </summary>
|
|
public void Resume()
|
|
{
|
|
if (_currentPath != null && _remainingNodes != null && _remainingNodes.Count > 0)
|
|
{
|
|
_isMoving = true;
|
|
SetState(AGVState.Moving);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - 프레임 업데이트 (외부에서 정기적으로 호출)
|
|
|
|
/// <summary>
|
|
/// 프레임 업데이트 (외부에서 주기적으로 호출)
|
|
/// 이 방식으로 타이머에 의존하지 않고 외부에서 제어 가능
|
|
/// </summary>
|
|
/// <param name="deltaTimeMs">마지막 업데이트 이후 경과 시간 (밀리초)</param>
|
|
public void Update(float deltaTimeMs)
|
|
{
|
|
var deltaTime = deltaTimeMs / 1000.0f; // 초 단위로 변환
|
|
|
|
UpdateMovement(deltaTime);
|
|
UpdateBattery(deltaTime);
|
|
|
|
// 위치 변경 이벤트 발생
|
|
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - 수동 제어 (테스트용)
|
|
|
|
/// <summary>
|
|
/// 수동 이동 (테스트용)
|
|
/// </summary>
|
|
/// <param name="targetPosition">목표 위치</param>
|
|
public void MoveTo(Point targetPosition)
|
|
{
|
|
_prevPosition = targetPosition;
|
|
_moveStartPosition = _currentPosition;
|
|
_moveTargetPosition = targetPosition;
|
|
|
|
SetState(AGVState.Moving);
|
|
_isMoving = true;
|
|
Turn = AGVTurn.None;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 수동 회전 (테스트용)
|
|
/// </summary>
|
|
/// <param name="direction">회전 방향</param>
|
|
public void Rotate(AgvDirection direction)
|
|
{
|
|
if (_currentState != AGVState.Idle)
|
|
return;
|
|
|
|
SetState(AGVState.Rotating);
|
|
_currentDirection = direction;
|
|
SetState(AGVState.Idle);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 충전 시작
|
|
/// </summary>
|
|
public void StartCharging()
|
|
{
|
|
if (_currentState == AGVState.Idle)
|
|
{
|
|
SetState(AGVState.Charging);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 충전 종료
|
|
/// </summary>
|
|
public void StopCharging()
|
|
{
|
|
if (_currentState == AGVState.Charging)
|
|
{
|
|
SetState(AGVState.Idle);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public Methods - AGV 위치 설정 (시뮬레이터용)
|
|
|
|
/// <summary>
|
|
/// AGV 위치 직접 설정
|
|
/// PrevPosition을 이전 위치로 저장하여 리프트 방향 계산이 가능하도록 함
|
|
/// </summary>
|
|
/// <param name="node">현재 노드</param>
|
|
/// <param name="newPosition">새로운 위치</param>
|
|
/// <param name="motorDirection">모터이동방향</param>
|
|
public void SetPosition(MapNode node, AgvDirection motorDirection)
|
|
{
|
|
// 현재 위치를 이전 위치로 저장 (리프트 방향 계산용)
|
|
if (_currentNode != null && _currentNode.Id != node.Id)
|
|
{
|
|
_prevPosition = _currentPosition; // 이전 위치
|
|
_prevNode = _currentNode;
|
|
_prevDirection = _currentDirection;
|
|
}
|
|
|
|
////모터방향이 다르다면 적용한다
|
|
//if (_currentDirection != motorDirection)
|
|
//{
|
|
// _prevDirection = motorDirection;
|
|
//}
|
|
|
|
// 새로운 위치 설정
|
|
_currentPosition = node.Position;
|
|
_currentDirection = motorDirection;
|
|
_currentNode = node;
|
|
|
|
// 🔥 노드 ID를 RFID로 간주하여 감지 목록에 추가 (시뮬레이터용)
|
|
if (!string.IsNullOrEmpty(node.Id) && !_detectedRfids.Contains(node.Id))
|
|
{
|
|
_detectedRfids.Add(node.Id);
|
|
}
|
|
|
|
// 🔥 RFID 2개 이상 감지 시 위치 확정
|
|
if (_detectedRfids.Count >= 2 && !_isPositionConfirmed)
|
|
{
|
|
_isPositionConfirmed = true;
|
|
}
|
|
|
|
//현재 경로값이 있는지 확인한다.
|
|
if (CurrentPath != null && CurrentPath.DetailedPath != null && CurrentPath.DetailedPath.Any())
|
|
{
|
|
var item = CurrentPath.DetailedPath.FirstOrDefault(t => t.NodeId == node.Id && t.IsPass == false);
|
|
if (item != null)
|
|
{
|
|
// [PathJump Check] 점프한 노드 개수 확인
|
|
// 현재 노드(item)보다 이전인데 아직 IsPass가 안 된 노드의 개수
|
|
int skippedCount = CurrentPath.DetailedPath.Count(t => t.seq < item.seq && t.IsPass == false);
|
|
if (skippedCount > 2)
|
|
{
|
|
OnError($"PathJump: {skippedCount}개의 노드를 건너뛰었습니다. (허용: 2개, 현재노드: {node.Id})");
|
|
return;
|
|
}
|
|
|
|
//item.IsPass = true;
|
|
//이전노드는 모두 지나친걸로 한다
|
|
CurrentPath.DetailedPath.Where(t => t.seq < item.seq).ToList().ForEach(t => t.IsPass = true);
|
|
}
|
|
}
|
|
|
|
// 위치 변경 이벤트 발생
|
|
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
|
|
}
|
|
|
|
|
|
#endregion
|
|
|
|
|
|
|
|
/// <summary>
|
|
/// 노드 ID를 RFID 값으로 변환 (NodeResolver 사용)
|
|
/// </summary>
|
|
public ushort GetRfidByNodeId(List<MapNode> _mapNodes, string nodeId)
|
|
{
|
|
var node = _mapNodes?.FirstOrDefault(n => n.Id == nodeId);
|
|
if ((node?.HasRfid() ?? false) == false) return 0;
|
|
return node.RfidId;
|
|
}
|
|
|
|
|
|
|
|
#region Private Methods
|
|
|
|
/// <summary>
|
|
/// DetailedPath에서 노드 정보를 찾아 AGVCommand 생성
|
|
/// </summary>
|
|
private AGVCommand GetCommandFromPath(MapNode targetNode, string actionDescription)
|
|
{
|
|
// DetailedPath가 없으면 기본 명령 반환
|
|
if (_currentPath == null || _currentPath.DetailedPath == null || _currentPath.DetailedPath.Count == 0)
|
|
{
|
|
// [Refactor] Predict와 일관성 유지: 경로가 없으면 정지
|
|
return new AGVCommand(
|
|
MotorCommand.Stop,
|
|
MagnetPosition.S,
|
|
SpeedLevel.L,
|
|
eAGVCommandReason.NoPath,
|
|
$"{actionDescription} (DetailedPath 없음)"
|
|
);
|
|
}
|
|
|
|
// DetailedPath에서 targetNodeId에 해당하는 NodeMotorInfo 찾기
|
|
// 지나가지 않은 경로를 찾는다
|
|
var nodeInfo = _currentPath.DetailedPath.FirstOrDefault(n => n.NodeId == targetNode.Id && n.IsPass == false);
|
|
|
|
if (nodeInfo == null)
|
|
{
|
|
// 못 찾으면 기본 명령 반환
|
|
var defaultMotor = _currentDirection == AgvDirection.Forward
|
|
? MotorCommand.Forward
|
|
: MotorCommand.Backward;
|
|
|
|
return new AGVCommand(
|
|
defaultMotor,
|
|
MagnetPosition.S,
|
|
SpeedLevel.M,
|
|
eAGVCommandReason.NoTarget,
|
|
$"{actionDescription} (노드 {targetNode.Id} 정보 없음)"
|
|
);
|
|
}
|
|
|
|
// MotorDirection → MotorCommand 변환
|
|
MotorCommand motorCmd;
|
|
switch (nodeInfo.MotorDirection)
|
|
{
|
|
case AgvDirection.Forward:
|
|
motorCmd = MotorCommand.Forward;
|
|
break;
|
|
case AgvDirection.Backward:
|
|
motorCmd = MotorCommand.Backward;
|
|
break;
|
|
default:
|
|
motorCmd = MotorCommand.Stop;
|
|
break;
|
|
}
|
|
|
|
// MagnetDirection → MagnetPosition 변换
|
|
MagnetPosition magnetPos;
|
|
switch (nodeInfo.MagnetDirection)
|
|
{
|
|
case MagnetDirection.Left:
|
|
magnetPos = MagnetPosition.L;
|
|
break;
|
|
case MagnetDirection.Right:
|
|
magnetPos = MagnetPosition.R;
|
|
break;
|
|
case MagnetDirection.Straight:
|
|
default:
|
|
magnetPos = MagnetPosition.S;
|
|
break;
|
|
}
|
|
|
|
// [Speed Control] NodeMotorInfo에 설정된 속도 사용
|
|
// 단, 회전 구간 등에서 안전을 위해 강제 감속이 필요한 경우 로직 추가 가능
|
|
// 현재는 사용자 설정 우선
|
|
SpeedLevel speed = nodeInfo.Speed;
|
|
|
|
// Optional: 회전 시 강제 감속 로직 (사용자 요청에 따라 주석 처리 또는 제거 가능)
|
|
// if (nodeInfo.CanRotate || nodeInfo.IsDirectionChangePoint) speed = SpeedLevel.L;
|
|
|
|
return new AGVCommand(
|
|
motorCmd,
|
|
magnetPos,
|
|
speed,
|
|
eAGVCommandReason.Normal,
|
|
$"{actionDescription} → {targetNode.Id} (Motor:{motorCmd}, Magnet:{magnetPos})"
|
|
);
|
|
}
|
|
|
|
private void StartMovement()
|
|
{
|
|
SetState(AGVState.Moving);
|
|
_isMoving = true;
|
|
_lastUpdateTime = DateTime.Now;
|
|
}
|
|
|
|
private void UpdateMovement(float deltaTime)
|
|
{
|
|
if (_currentState != AGVState.Moving || !_isMoving)
|
|
return;
|
|
|
|
// 목표 위치까지의 거리 계산
|
|
var distance = CalculateDistance(_currentPosition, _moveTargetPosition);
|
|
|
|
if (distance < 5.0f) // 도달 임계값
|
|
{
|
|
// 목표 도달
|
|
_currentPosition = _moveTargetPosition;
|
|
_currentSpeed = 0;
|
|
|
|
// 다음 노드로 이동
|
|
ProcessNextNode();
|
|
}
|
|
else
|
|
{
|
|
// 계속 이동
|
|
var moveDistance = _moveSpeed * deltaTime;
|
|
var direction = new PointF(
|
|
_moveTargetPosition.X - _currentPosition.X,
|
|
_moveTargetPosition.Y - _currentPosition.Y
|
|
);
|
|
|
|
// 정규화
|
|
var length = (float)Math.Sqrt(direction.X * direction.X + direction.Y * direction.Y);
|
|
if (length > 0)
|
|
{
|
|
direction.X /= length;
|
|
direction.Y /= length;
|
|
}
|
|
|
|
// 새 위치 계산
|
|
_currentPosition = new Point(
|
|
(int)(_currentPosition.X + direction.X * moveDistance),
|
|
(int)(_currentPosition.Y + direction.Y * moveDistance)
|
|
);
|
|
|
|
_currentSpeed = _moveSpeed;
|
|
}
|
|
}
|
|
|
|
private void UpdateBattery(float deltaTime)
|
|
{
|
|
// 배터리 소모 시뮬레이션
|
|
if (_currentState == AGVState.Moving)
|
|
{
|
|
BatteryLevel -= 0.1f * deltaTime; // 이동시 소모
|
|
}
|
|
else if (_currentState == AGVState.Charging)
|
|
{
|
|
BatteryLevel += 5.0f * deltaTime; // 충전
|
|
BatteryLevel = Math.Min(100.0f, BatteryLevel);
|
|
}
|
|
|
|
BatteryLevel = Math.Max(0, BatteryLevel);
|
|
}
|
|
|
|
public MapNode StartNode { get; set; } = null;
|
|
|
|
private MapNode _targetnode = null;
|
|
|
|
/// <summary>
|
|
/// 목적지를 설정합니다. 목적지가 변경되면 경로계산정보가 삭제 됩니다.
|
|
/// </summary>
|
|
public MapNode TargetNode
|
|
{
|
|
get
|
|
{
|
|
return _targetnode;
|
|
}
|
|
set
|
|
{
|
|
if (_targetnode != value)
|
|
{
|
|
_currentPath = null;
|
|
_targetnode = value;
|
|
}
|
|
}
|
|
}
|
|
|
|
private void ProcessNextNode()
|
|
{
|
|
if (_remainingNodes == null || _currentNodeIndex >= _remainingNodes.Count - 1)
|
|
{
|
|
// 경로 완료
|
|
_isMoving = false;
|
|
SetState(AGVState.Idle);
|
|
PathCompleted?.Invoke(this, _currentPath);
|
|
return;
|
|
}
|
|
|
|
// 다음 노드로 이동
|
|
_currentNodeIndex++;
|
|
var nextNodeId = _remainingNodes[_currentNodeIndex];
|
|
|
|
// RFID 감지 시뮬레이션
|
|
RfidDetected?.Invoke(this, $"RFID_{nextNodeId}");
|
|
|
|
// 다음 목표 위치 설정 (실제로는 맵에서 좌표 가져와야 함)
|
|
var random = new Random();
|
|
_moveTargetPosition = new Point(
|
|
_currentPosition.X + random.Next(-100, 100),
|
|
_currentPosition.Y + random.Next(-100, 100)
|
|
);
|
|
}
|
|
|
|
private MapNode FindClosestNode(Point position, List<MapNode> mapNodes)
|
|
{
|
|
if (mapNodes == null || mapNodes.Count == 0)
|
|
return null;
|
|
|
|
MapNode closestNode = null;
|
|
float closestDistance = float.MaxValue;
|
|
|
|
foreach (var node in mapNodes)
|
|
{
|
|
var distance = CalculateDistance(position, node.Position);
|
|
if (distance < closestDistance)
|
|
{
|
|
closestDistance = distance;
|
|
closestNode = node;
|
|
}
|
|
}
|
|
|
|
return closestDistance < 50.0f ? closestNode : null;
|
|
}
|
|
|
|
private float CalculateDistance(Point from, Point to)
|
|
{
|
|
var dx = to.X - from.X;
|
|
var dy = to.Y - from.Y;
|
|
return (float)Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
|
|
private void SetState(AGVState newState)
|
|
{
|
|
if (_currentState != newState)
|
|
{
|
|
_currentState = newState;
|
|
StateChanged?.Invoke(this, newState);
|
|
}
|
|
}
|
|
|
|
|
|
private void OnError(string message)
|
|
{
|
|
SetState(AGVState.Error);
|
|
ErrorOccurred?.Invoke(this, message);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Cleanup
|
|
|
|
/// <summary>
|
|
/// 리소스 정리
|
|
/// </summary>
|
|
public void Dispose()
|
|
{
|
|
StopPath();
|
|
}
|
|
|
|
#endregion
|
|
}
|
|
}
|