턴동작 추가
This commit is contained in:
backuppc
@@ -21,6 +21,9 @@
|
|||||||
/// <summary>명령 이유- (디버깅/로깅용)</summary>
|
/// <summary>명령 이유- (디버깅/로깅용)</summary>
|
||||||
public eAGVCommandReason Reason { get; set; }
|
public eAGVCommandReason Reason { get; set; }
|
||||||
|
|
||||||
|
/// <summary>방향 전환 명령 여부 (180도 Left Turn 등)</summary>
|
||||||
|
public bool IsTurn { get; set; }
|
||||||
|
|
||||||
/// <summary>
|
/// <summary>
|
||||||
/// 생성자
|
/// 생성자
|
||||||
/// </summary>
|
/// </summary>
|
||||||
|
|||||||
@@ -720,24 +720,34 @@ namespace AGVNavigationCore.Models
|
|||||||
|
|
||||||
// MotorDirection → MotorCommand 변환
|
// MotorDirection → MotorCommand 변환
|
||||||
MotorCommand motorCmd;
|
MotorCommand motorCmd;
|
||||||
switch (nodeInfo.MotorDirection)
|
eAGVCommandReason reason = eAGVCommandReason.Normal;
|
||||||
|
|
||||||
|
if (nodeInfo.IsTurn)
|
||||||
{
|
{
|
||||||
case AgvDirection.Forward:
|
motorCmd = MotorCommand.Stop;
|
||||||
motorCmd = MotorCommand.Forward;
|
reason = eAGVCommandReason.MarkStop;
|
||||||
break;
|
}
|
||||||
case AgvDirection.Backward:
|
else
|
||||||
motorCmd = MotorCommand.Backward;
|
{
|
||||||
break;
|
switch (nodeInfo.MotorDirection)
|
||||||
default:
|
{
|
||||||
motorCmd = MotorCommand.Stop;
|
case AgvDirection.Forward:
|
||||||
break;
|
motorCmd = MotorCommand.Forward;
|
||||||
|
break;
|
||||||
|
case AgvDirection.Backward:
|
||||||
|
motorCmd = MotorCommand.Backward;
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
motorCmd = MotorCommand.Stop;
|
||||||
|
break;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
// MagnetDirection → MagnetPosition 변换
|
// MagnetDirection → MagnetPosition 변换
|
||||||
MagnetPosition magnetPos;
|
MagnetPosition magnetPos;
|
||||||
switch (nodeInfo.MagnetDirection)
|
switch (nodeInfo.MagnetDirection)
|
||||||
{
|
{
|
||||||
case MagnetDirection.Left:
|
case MagnetDirection.Left:
|
||||||
magnetPos = MagnetPosition.L;
|
magnetPos = MagnetPosition.L;
|
||||||
break;
|
break;
|
||||||
case MagnetDirection.Right:
|
case MagnetDirection.Right:
|
||||||
@@ -761,9 +771,12 @@ namespace AGVNavigationCore.Models
|
|||||||
motorCmd,
|
motorCmd,
|
||||||
magnetPos,
|
magnetPos,
|
||||||
speed,
|
speed,
|
||||||
eAGVCommandReason.Normal,
|
reason,
|
||||||
$"{actionDescription} → {targetNode.Id} (Motor:{motorCmd}, Magnet:{magnetPos})"
|
$"{actionDescription} → {targetNode.Id} (Motor:{motorCmd}, Magnet:{magnetPos})"
|
||||||
);
|
)
|
||||||
|
{
|
||||||
|
IsTurn = nodeInfo.IsTurn
|
||||||
|
};
|
||||||
}
|
}
|
||||||
|
|
||||||
private void StartMovement()
|
private void StartMovement()
|
||||||
|
|||||||
@@ -236,6 +236,12 @@ namespace Project
|
|||||||
// 완료(Complete) 상태라면 MarkStop 전송
|
// 완료(Complete) 상태라면 MarkStop 전송
|
||||||
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
|
if (nextAction.Reason == AGVNavigationCore.Models.eAGVCommandReason.MarkStop)
|
||||||
{
|
{
|
||||||
|
// 턴 중이거나 턴 동작을 방금 명령했다면 MarkStop 전송 생략
|
||||||
|
if (nextAction.IsTurn && PUB.AGV.TurnInformation.State == arDev.eNarumiTurn.LeftIng)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (PUB.AGV.data.Speed != 'S')
|
if (PUB.AGV.data.Speed != 'S')
|
||||||
{
|
{
|
||||||
//2초간격으로 명령을 전송한다
|
//2초간격으로 명령을 전송한다
|
||||||
@@ -267,6 +273,47 @@ namespace Project
|
|||||||
}
|
}
|
||||||
|
|
||||||
//여기시점에서는 장비는 멈춘 상태이다
|
//여기시점에서는 장비는 멈춘 상태이다
|
||||||
|
if (nextAction.IsTurn)
|
||||||
|
{
|
||||||
|
// 1. 턴 완료 상태 확인
|
||||||
|
if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.Left)
|
||||||
|
{
|
||||||
|
var tsTurn = VAR.TIME.RUN(eVarTime.LastTurnCommandTime);
|
||||||
|
// 턴 동작 미진행 (또는 이전 명령이 무시된 경우, 쿨타임 5초)
|
||||||
|
if (PUB.AGV.TurnInformation.State != arDev.eNarumiTurn.LeftIng && tsTurn.TotalSeconds > 5)
|
||||||
|
{
|
||||||
|
PUB.log.Add($"[경로실행] 180도 좌회전 명령 전송");
|
||||||
|
PUB.AGV.AGVMoveLeft180Turn();
|
||||||
|
VAR.TIME.Update(eVarTime.LastTurnCommandTime);
|
||||||
|
}
|
||||||
|
else if (PUB.AGV.TurnInformation.State == arDev.eNarumiTurn.LeftIng)
|
||||||
|
{
|
||||||
|
// 턴 진행 중 (타임아웃은 30초)
|
||||||
|
if (tsTurn.TotalSeconds > 30)
|
||||||
|
{
|
||||||
|
SetRunStepError(ENIGProtocol.AGVErrorCode.TURN_FAIL, "경로 중 턴 동작 대기 타임아웃 (30초)");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
PUB._mapCanvas.SetInfoMessage($"턴 진행 중...({tsTurn.TotalSeconds:F1}/30)");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 턴이 완료될 때까지 대기 반환
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 2. 턴 정상 완료
|
||||||
|
PUB.log.Add($"[경로실행] 180도 좌회전 완료 확인. 턴 명령 노드 패스 처리");
|
||||||
|
PUB._virtualAGV.SetCurrentNodeMarkStop(); // 첫번째 미완료 노드(IsTurn=true인 노드)를 Pass로 전환
|
||||||
|
|
||||||
|
// 다음 루프 명령 실행 시 영향 없도록 Turn State 초기화
|
||||||
|
PUB.AGV.TurnInformation.State = arDev.eNarumiTurn.None;
|
||||||
|
|
||||||
|
// 이번 이동 함수 종료, 다음 루프에서 갱신된 Predict 호출
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
if (nextAction.Reason == eAGVCommandReason.Complete)
|
if (nextAction.Reason == eAGVCommandReason.Complete)
|
||||||
{
|
{
|
||||||
// 목적지 도착 여부 확인
|
// 목적지 도착 여부 확인
|
||||||
@@ -311,7 +358,6 @@ namespace Project
|
|||||||
//PREDICT에서 이동을 명령했따
|
//PREDICT에서 이동을 명령했따
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
var bunki = arDev.Narumi.eBunki.Strate;
|
var bunki = arDev.Narumi.eBunki.Strate;
|
||||||
if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
|
if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.L) bunki = arDev.Narumi.eBunki.Left;
|
||||||
else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;
|
else if (nextAction.Magnet == AGVNavigationCore.Models.MagnetPosition.R) bunki = arDev.Narumi.eBunki.Right;
|
||||||
|
|||||||
109
HMI/docs/pathtest.md
Normal file
109
HMI/docs/pathtest.md
Normal file
@@ -0,0 +1,109 @@
|
|||||||
|
# AGV 하드코딩 경로 실행 타이밍 차트 및 동작 분석
|
||||||
|
|
||||||
|
본 문서는 `7B -> 11B -> 3T -> 3B -> 71B` 와 같은 연속된 조향/턴 하드코딩 경로가 주어졌을 때,
|
||||||
|
현재 구현된 `AGVPathfinder`, `VirtualAGV`, `_Util.cs` 시스템 상에서 AGV가 시간 흐름에 따라 어떠한 동작과 판단을 거치는지 정리한 타이밍 시퀀스 분석입니다.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 경로 정의 및 노드 정보
|
||||||
|
|
||||||
|
| Seq | Tag | Node ID | 모터 방향 | IsTurn | 동작 목적 |
|
||||||
|
|---|---|---|---|---|---|
|
||||||
|
| 1 | 7B | Node 7 | Backward | false | 시작점, 후진 출발 |
|
||||||
|
| 2 | 11B| Node 11| Backward | false | 단순 통과(Pass) 노드 |
|
||||||
|
| 3 | 3T | Node 3 | Stop | true | 180도 좌회전 (Turn) |
|
||||||
|
| 4 | 3B | Node 3 | Backward | false | 턴 직후의 후진 방향 정렬 |
|
||||||
|
| 5 | 71B| Node 71| Stop | false | 최종 목적지 (Loader), 정차 |
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## Execution Sequence (실행 시퀀스)
|
||||||
|
|
||||||
|
### 1단계: Node 7 출발 (7B)
|
||||||
|
- **상태:** AGV가 7번 노드에 위치함.
|
||||||
|
- **SetPosition:** `VirtualAGV`는 현재 노드를 7로 설정.
|
||||||
|
- **Predict() 판단:**
|
||||||
|
- 현재 노드 정보(`7B`)를 기반으로 다음 미완료 노드를 11번으로 추적.
|
||||||
|
- 리턴: `MotorCommand=Backward`, `Speed=Low(or M)`, `Reason=Normal`.
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- AGV가 정지 상태(`agv_run == false`)이므로, 구동 명령 분기로 진입.
|
||||||
|
- `AGVMoveSet` 설정 및 `AGVMoveRun(Backward)` 명령 전송.
|
||||||
|
- AGV 후진 이동 시작.
|
||||||
|
|
||||||
|
### 2단계: Node 11 통과 (11B)
|
||||||
|
- **상태:** AGV가 후진 중 11번 노드의 RFID 태그를 읽음.
|
||||||
|
- **SetPosition:** `VirtualAGV`는 현재 노드를 11로 갱신.
|
||||||
|
- (이전 노드인 `7B` 노드는 `IsPass=true` 로 자동 패스 처리됨)
|
||||||
|
- **Predict() 판단:**
|
||||||
|
- 다음 미완료 타겟인 `11B` 노드 방향(`Backward`)을 리턴.
|
||||||
|
- 리턴: `MotorCommand=Backward`, `Reason=Normal`.
|
||||||
|
- (*참고: 일반 주행 노드는 밟고 즉시 정지하는 것이 아니라 통과하는 것이므로, 다음 태그를 밟을 때까지 계속 이동 상태를 유지합니다.*)
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- 상태 변화 없이 계속 후진 이동(Backward).
|
||||||
|
|
||||||
|
### 3단계: Node 3 도착 및 MarkStop 타겟 인식 (3T)
|
||||||
|
- **상태:** AGV가 후진을 계속하여 노드 3 RFID 태그를 읽음.
|
||||||
|
- **SetPosition:** `VirtualAGV`는 현재 노드를 3으로 갱신.
|
||||||
|
- (이때 `11B`까지의 이전 노드들이 모두 `IsPass=true` 처리됨)
|
||||||
|
- **Predict() 판단:**
|
||||||
|
- 현재 평가 대상(타겟) 노드가 비로소 `3T`(`IsTurn=true`)가 됨.
|
||||||
|
- `IsTurn=true` 노드는 통과가 아닌 물리적 로직(정지/턴)이 필요하므로:
|
||||||
|
- 리턴: `MotorCommand=Stop`, `Reason=MarkStop`, `IsTurn=true`.
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- AGV는 아직 이동 중(`agv_run == true`).
|
||||||
|
- `Reason == MarkStop` 분기로 진입.
|
||||||
|
- `IsTurn == true` 이지만 실제로 돌고 있지는 않으므로, 정학한 위치 정지를 위해 `AGVMoveStop(MarkStop)` 명령 전송.
|
||||||
|
|
||||||
|
### 4단계: 감속 및 턴 대기
|
||||||
|
- **상태:** AGV가 마크 센터에 맞추어 감속 후 멈춤.
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- `agv_run == true` 기간 동안 아무 추가 명령 없이 대기.
|
||||||
|
|
||||||
|
### 5단계: 정지 완료 및 180도 턴 시작
|
||||||
|
- **상태:** `agv_run == false` (AGV 모터 완전 정지 상태 갱신).
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- 정지 상태 루트로 진입.
|
||||||
|
- `IsTurn == true` 이므로 턴 로직 블럭 진입.
|
||||||
|
- `TurnInformation.State != Left` 임을 확인.
|
||||||
|
- `AGVMoveLeft180Turn()` 명령 전송.
|
||||||
|
- **AGV 실제 동작:**
|
||||||
|
- 그 자리에서 180도 위치 회전 수행.
|
||||||
|
|
||||||
|
### 6단계: 턴 로직 구동 중
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- AGV가 도는 동안 `TurnInformation.State == LeftIng` 로 유지.
|
||||||
|
- 매 틱마다 "턴 진행 중..." 메시지를 띄우고 대기(Return false). 일정 시간(30초) 타임아웃 검사 체제 가동.
|
||||||
|
|
||||||
|
### 7단계: 턴 완료 및 노드 패스 처리 (3T -> 3B 전환)
|
||||||
|
- **상태:** AGV 180도 회전 완료. HMI 쪽으로 완료 패킷 수신됨.
|
||||||
|
- `TurnInformation.State = Left` 상태로 갱신.
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- 턴 완료 상태를 인지하고 패스 로직 수행.
|
||||||
|
- `PUB._virtualAGV.SetCurrentNodeMarkStop()` 호출. (이 함수는 현재 가장 먼저 미완료된 `3T` 노드를 강제로 `IsPass=true` 처리하는 역할).
|
||||||
|
- 다음 루프 영향을 막고자 `TurnInformation.State = None` 초기화 후 대기 리턴.
|
||||||
|
|
||||||
|
### 8단계: 턴 완료 후 3B 방향 이동 재개
|
||||||
|
- **Predict() 판단 (다음 틱):**
|
||||||
|
- 이제 `3T`가 `IsPass=true` 가 되었기 때문에 타겟은 `3B` 로 변경됨.
|
||||||
|
- 리턴: `MotorCommand=Backward`, `IsTurn=false`. (AGV 차체가 180도 돌았으므로 시스템상 방향이 반전되어 있어, 다시 후진 명령이 하달됨)
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- `agv_run == false` 상태.
|
||||||
|
- `MoveSet`(방향: Backward) 설정 후, `AGVMoveRun(Backward)` 전송.
|
||||||
|
- **AGV 실제 동작:**
|
||||||
|
- 후진 방향으로(이번에는 노드 71을 향해) 기동 시작.
|
||||||
|
|
||||||
|
### 9단계: 목적지 71 도착 및 완료 (71B)
|
||||||
|
- **상태:** 후진 이동 후 71번 노드(Loader) 태그 인식.
|
||||||
|
- **SetPosition:** 71 갱신. 이전 `3B` 노드 자동 `IsPass=true`.
|
||||||
|
- **Predict() 판단:**
|
||||||
|
- 타겟은 배열의 최후 노드 `71B`.
|
||||||
|
- 마지막 노드에 도착했음을 감지하여 리턴: `MotorCommand=Stop`, `Reason=MarkStop`.
|
||||||
|
- **_Util.cs 처리:**
|
||||||
|
- 이동 중이므로 `MarkStop` 명령 전송하여 도킹 위치 정밀 정차 유도.
|
||||||
|
- 완전 정지(`agv_run == false`) 후, `Complete` 판단 조건에 따라 경로 작업 시퀀스 종료 및 다음 태스크(`PickOnEnter` 등)로 이동.
|
||||||
|
|
||||||
|
---
|
||||||
|
|
||||||
|
## 결론
|
||||||
|
새롭게 개선된 로직에서는 **`VirtualAGV`가 `3T` 와 같은 `IsTurn` 포인트를 읽어 다음 이동 목표로 할당할지라도 즉시 통과시키지 않고 MarkStop을 유도**합니다.
|
||||||
|
AGV가 해당 노드에서 완벽히 멈춘 상태를 HMI(`_Util.cs`)가 확인한 후 180도 턴을 시도하며, 턴의 물리적 완료 피드백을 수신하고 나서야 해당 턴 명령(`3T`)을 `Pass` 판정 지음으로써 완벽한 시퀀스 제어무결성을 보장합니다.
|
||||||
Reference in New Issue
Block a user