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

feat: Implement AGV command prediction system for real-time control

Browse Source 
- Add motor/magnet/speed enums and AGVCommand class for AGV control
- Implement Predict() method for next action prediction based on path and state
- Add RFID position tracking (requires 2 RFIDs for position confirmation)
- Add SetPath() method to VirtualAGV for path management
- Implement GetCommandFromPath() to extract motor/magnet/speed from DetailedPath
- Add real-time prediction display in SimulatorForm (timer1_Tick)
- Support automatic forward movement at low speed when position unconfirmed
- Support stop command when destination reached or no destination set

Key Features:
- Position unconfirmed (RFID < 2): Forward + Straight + Low speed
- Position confirmed + no destination: Stop (position found)
- Position confirmed + destination reached: Stop (arrived)
- Path execution: Motor/Magnet/Speed from DetailedPath NodeMotorInfo
- Rotation nodes: Automatic low speed
- Junction handling: Magnet direction (Left/Right/Straight)

🤖 Generated with [Claude Code](https://claude.com/claude-code)

Co-Authored-By: Claude <noreply@anthropic.com>
This commit is contained in:
backuppc
2025-11-03 17:34:35 +09:00
parent c3f8c7117d
commit 3712c290c9
7 changed files with 555 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
Cs_HMI/AGVLogic/AGVNavigationCore/Models/Enums.cs
View File

@@ -69,4 +69,90 @@ namespace AGVNavigationCore.Models
/// <summary>충전기</summary>
Charger
}
/// <summary>
/// 모터 명령 열거형 (실제 AGV 제어용)
/// </summary>
public enum MotorCommand
{
/// <summary>정지</summary>
Stop,
/// <summary>전진 (Forward - 모니터 방향)</summary>
Forward,
/// <summary>후진 (Backward - 리프트 방향)</summary>
Backward
}
/// <summary>
/// 마그넷 위치 열거형 (실제 AGV 제어용)
/// </summary>
public enum MagnetPosition
{
/// <summary>직진 (Straight)</summary>
S,
/// <summary>왼쪽 (Left)</summary>
L,
/// <summary>오른쪽 (Right)</summary>
R
}
/// <summary>
/// 속도 레벨 열거형 (실제 AGV 제어용)
/// </summary>
public enum SpeedLevel
{
/// <summary>저속 (Low)</summary>
L,
/// <summary>중속 (Medium)</summary>
M,
/// <summary>고속 (High)</summary>
H
}
/// <summary>
/// AGV 제어 명령 클래스 (실제 AGV 제어용)
/// Predict() 메서드가 반환하는 다음 동작 명령
/// </summary>
public class AGVCommand
{
/// <summary>모터 명령 (정지/전진/후진)</summary>
public MotorCommand Motor { get; set; }
/// <summary>마그넷 위치 (직진/왼쪽/오른쪽)</summary>
public MagnetPosition Magnet { get; set; }
/// <summary>속도 레벨 (저속/중속/고속)</summary>
public SpeedLevel Speed { get; set; }
/// <summary>명령 이유 (디버깅/로깅용)</summary>
public string Reason { get; set; }
/// <summary>
/// 생성자
/// </summary>
public AGVCommand(MotorCommand motor, MagnetPosition magnet, SpeedLevel speed, string reason = "")
{
Motor = motor;
Magnet = magnet;
Speed = speed;
Reason = reason;
}
/// <summary>
/// 기본 생성자
/// </summary>
public AGVCommand()
{
Motor = MotorCommand.Stop;
Magnet = MagnetPosition.S;
Speed = SpeedLevel.L;
Reason = "";
}
public override string ToString()
{
return $"Motor:{Motor}, Magnet:{Magnet}, Speed:{Speed}" +
(string.IsNullOrEmpty(Reason) ? "" : $" ({Reason})");
}
}
}

327
Cs_HMI/AGVLogic/AGVNavigationCore/Models/VirtualAGV.cs
View File

@@ -75,6 +75,10 @@ namespace AGVNavigationCore.Models
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개 이상 감지)
#endregion
#region Properties
@@ -159,6 +163,16 @@ namespace AGVNavigationCore.Models
/// </summary>
public DockingDirection DockingDirection => _dockingDirection;
/// <summary>
/// 위치 확정 여부 (RFID 2개 이상 감지 시 true)
/// </summary>
public bool IsPositionConfirmed => _isPositionConfirmed;
/// <summary>
/// 감지된 RFID 개수
/// </summary>
public int DetectedRfidCount => _detectedRfids.Count;
#endregion
#region Constructor
@@ -200,6 +214,18 @@ namespace AGVNavigationCore.Models
/// </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);
}
@@ -225,6 +251,172 @@ namespace AGVNavigationCore.Models
}
}
/// <summary>
/// 다음 동작 예측 (실제 AGV 제어용)
/// AGV가 지속적으로 호출하여 현재 상태와 예측 상태를 일치시킴
/// </summary>
/// <returns>다음에 수행할 모터/마그넷/속도 명령</returns>
public AGVCommand Predict()
{
// 1. 위치 미확정 상태 (RFID 2개 미만 감지)
if (!_isPositionConfirmed)
{
// 항상 전진 + 저속으로 이동 (RFID 감지 대기)
return new AGVCommand(
MotorCommand.Forward,
MagnetPosition.S, // 직진
SpeedLevel.L, // 저속
$"위치 미확정 (RFID {_detectedRfids.Count}/2) - 전진하여 RFID 탐색"
);
}
// 2. 위치 확정됨 + 경로 없음 → 정지 (목적지 미설정 상태)
if (_currentPath == null || (_currentPath.DetailedPath?.Count ?? 0) < 1)
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
$"위치 확정 완료 (목적지 미설정) - 현재:{_currentNode?.NodeId ?? "알수없음"}"
);
}
// 3. 위치 확정됨 + 경로 있음 + 남은 노드 없음 → 정지 (목적지 도착)
var lastNode = _currentPath.DetailedPath.Last();
if (_currentPath.DetailedPath.Where(t => t.seq < lastNode.seq && t.IsPass == false).Any() == false)
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
$"목적지 도착 - 최종:{_currentNode?.NodeId ?? "알수없음"}"
);
}
// 4. 경로이탈
var TargetNode = _currentPath.DetailedPath.Where(t => t.IsPass == false && t.NodeId.Equals(_currentNode.NodeId)).FirstOrDefault();
if (TargetNode == null)
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
$"(재탐색요청)경로이탈 현재위치:{_currentNode.NodeId}"
);
}
// 5. 방향체크
if(CurrentDirection != TargetNode.MotorDirection)
{
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
$"(재탐색요청)모터방향 불일치 현재위치:{_currentNode.NodeId}"
);
}
//this.CurrentNodeId
return GetCommandFromPath(CurrentNodeId, "경로 실행 시작");
// 4. 위치 확정 + 경로 실행 중 → 현재 상태에 따른 명령 예측
switch (_currentState)
{
case AGVState.Idle:
// 🔥 경로가 있다면 이동 시작 (경로 실행 대기 중)
if (_currentPath != null && _remainingNodes != null && _remainingNodes.Count > 0)
{
// DetailedPath에서 다음 노드 정보 가져오기
return GetCommandFromPath(_remainingNodes[0], "경로 실행 시작");
}
// 경로가 없으면 대기
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
"대기 중 (경로 없음)"
);
case AGVState.Moving:
{
// 이동 중 - DetailedPath에서 현재/다음 노드 정보 가져오기
if (_currentPath != null && _remainingNodes != null && _remainingNodes.Count > 0)
{
return GetCommandFromPath(_remainingNodes[0], "이동 중");
}
// 경로 정보가 없으면 기본 명령 (fallback)
var motorCmd = _currentDirection == AgvDirection.Forward
? MotorCommand.Forward
: MotorCommand.Backward;
return new AGVCommand(
motorCmd,
MagnetPosition.S,
SpeedLevel.M,
$"이동 중 (DetailedPath 없음)"
);
}
case AGVState.Rotating:
// 회전 중 - 정지 상태에서 마그넷만 조정
MagnetPosition magnetPos = MagnetPosition.S;
if (_currentDirection == AgvDirection.Left)
magnetPos = MagnetPosition.L;
else if (_currentDirection == AgvDirection.Right)
magnetPos = MagnetPosition.R;
return new AGVCommand(
MotorCommand.Stop, // 회전은 정지 상태에서
magnetPos,
SpeedLevel.L,
$"회전 중 ({_currentDirection})"
);
case AGVState.Docking:
{
// 도킹 중 - 저속으로 전진 또는 후진
var dockingMotor = _dockingDirection == DockingDirection.Forward
? MotorCommand.Forward
: MotorCommand.Backward;
return new AGVCommand(
dockingMotor,
MagnetPosition.S,
SpeedLevel.L, // 도킹은 항상 저속
$"도킹 중 ({_dockingDirection})"
);
}
case AGVState.Charging:
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
"충전 중"
);
case AGVState.Error:
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
"오류 발생"
);
default:
return new AGVCommand(
MotorCommand.Stop,
MagnetPosition.S,
SpeedLevel.L,
"알 수 없는 상태"
);
}
}
#endregion
#region Public Methods - 상태 조회
@@ -258,8 +450,29 @@ namespace AGVNavigationCore.Models
#region Public Methods - 경로 실행
/// <summary>
/// 경로 설정 (실제 AGV 및 시뮬레이터에서 사용)
/// </summary>
/// <param name="path">실행할 경로</param>
public void SetPath(AGVPathResult path)
{
if (path == null)
{
OnError("경로가 null입니다.");
return;
}
_currentPath = path;
_remainingNodes = path.Path.Select(n => n.NodeId).ToList(); // MapNode → NodeId 변환
_currentNodeIndex = 0;
// 경로 시작 노드가 현재 노드와 다른 경우 경고
if (_currentNode != null && _remainingNodes.Count > 0 && _remainingNodes[0] != _currentNode.NodeId)
{
OnError($"경로 시작 노드({_remainingNodes[0]})와 현재 노드({_currentNode.NodeId})가 다릅니다.");
}
}
/// <summary>
/// 경로 정지
/// </summary>
@@ -388,6 +601,31 @@ namespace AGVNavigationCore.Models
_currentDirection = motorDirection;
_currentNode = node;
// 🔥 노드 ID를 RFID로 간주하여 감지 목록에 추가 (시뮬레이터용)
if (!string.IsNullOrEmpty(node.NodeId) && !_detectedRfids.Contains(node.NodeId))
{
_detectedRfids.Add(node.NodeId);
}
// 🔥 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.NodeId && t.IsPass == false);
if (item != null)
{
//item.IsPass = true;
//이전노드는 모두 지나친걸로 한다
CurrentPath.DetailedPath.Where(t => t.seq < item.seq).ToList().ForEach(t => t.IsPass = true);
}
}
// 위치 변경 이벤트 발생
PositionChanged?.Invoke(this, (_currentPosition, _currentDirection, _currentNode));
}
@@ -408,6 +646,89 @@ namespace AGVNavigationCore.Models
#region Private Methods
/// <summary>
/// DetailedPath에서 노드 정보를 찾아 AGVCommand 생성
/// </summary>
private AGVCommand GetCommandFromPath(string targetNodeId, string actionDescription)
{
// DetailedPath가 없으면 기본 명령 반환
if (_currentPath == null || _currentPath.DetailedPath == null || _currentPath.DetailedPath.Count == 0)
{
var defaultMotor = _currentDirection == AgvDirection.Forward
? MotorCommand.Forward
: MotorCommand.Backward;
return new AGVCommand(
defaultMotor,
MagnetPosition.S,
SpeedLevel.M,
$"{actionDescription} (DetailedPath 없음)"
);
}
// DetailedPath에서 targetNodeId에 해당하는 NodeMotorInfo 찾기
// 지나가지 않은 경로를 찾는다
var nodeInfo = _currentPath.DetailedPath.FirstOrDefault(n => n.NodeId == targetNodeId && n.IsPass == false);
if (nodeInfo == null)
{
// 못 찾으면 기본 명령 반환
var defaultMotor = _currentDirection == AgvDirection.Forward
? MotorCommand.Forward
: MotorCommand.Backward;
return new AGVCommand(
defaultMotor,
MagnetPosition.S,
SpeedLevel.M,
$"{actionDescription} (노드 {targetNodeId} 정보 없음)"
);
}
// 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 PathFinding.Planning.MagnetDirection.Left:
magnetPos = MagnetPosition.L;
break;
case PathFinding.Planning.MagnetDirection.Right:
magnetPos = MagnetPosition.R;
break;
case PathFinding.Planning.MagnetDirection.Straight:
default:
magnetPos = MagnetPosition.S;
break;
}
// 속도 결정 (회전 노드면 저속, 일반 이동은 중속)
SpeedLevel speed = nodeInfo.CanRotate || nodeInfo.IsDirectionChangePoint
? SpeedLevel.L
: SpeedLevel.M;
return new AGVCommand(
motorCmd,
magnetPos,
speed,
$"{actionDescription} → {targetNodeId} (Motor:{motorCmd}, Magnet:{magnetPos})"
);
}
private void StartMovement()
{
SetState(AGVState.Moving);
@@ -559,7 +880,7 @@ namespace AGVNavigationCore.Models
#endregion
#region Cleanup