Implement ACS Command Handlers (PickOn, PickOff, Charge), Manual Mode Safety, and Map UI Commands

This commit is contained in:
2025-12-13 02:40:55 +09:00
parent 703e1387bf
commit 34b038c4be
25 changed files with 1992 additions and 295 deletions

View File

@@ -0,0 +1,78 @@
# 충전 시퀀스 분석 (Charging Sequence Analysis)
## 1. 개요 (Overview)
이 문서는 AGV의 자동 및 수동 충전 프로세스에 대한 분석 결과를 기술합니다. UI 트리거부터 이동, 도킹, 충전 명령 전송 및 확인까지의 전체 흐름을 포함합니다.
## 2. UI 트리거 (UI Trigger)
* **관련 컨트롤**: `btCharge` (자동충전/해제), `btChargeM` (수동충전 모드 토글)
* **이벤트 핸들러**: `btCharge_Click`, `btChargeM_Click` (in `fMain.cs`)
* **동작 로직**:
* **자동 충전 (`btCharge`)**:
* 이미 충전 관련 상태(`GOCHARGE`, `CHARGECHECK`)이거나 충전 중(`FLAG_CHARGEONA`)인 경우: → 충전 중지 여부 확인 후 `GOTO`(이동) 상태로 전환하여 충전 해제.
* 대기 상태인 경우: → 충전 시작 여부 확인 후 `GOCHARGE` RunStep 설정 및 `RUN` 상태 시작.
* **수동 충전 (`btChargeM`)**:
* `FLAG_CHARGEONM` 플래그를 토글합니다. 자동 충전 중일 때는 진입 불가.
## 3. 상태 머신 로직 (State Machine Logic)
### 3.1 충전 이동 (`_SM_RUN_GOCHARGE.cs`)
충전기를 찾아 이동하고 도킹하는 핵심 시퀀스입니다.
**사전 검사 (Prerequisites)**
1. **초기화**: `PUB.Result.CurrentPos` 초기화.
2. **하드웨어 체크**: AGV 연결 상태 확인.
3. **충전 상태 확인 (최적화)**: 이미 충전 중(`FLAG_CHARGEONA` or `Battery_charging`)인 경우, 충전 시퀀스를 진행하지 않고 즉시 `READY` 상태로 전환합니다. (불필요한 충전 해제/재시작 방지).
4. **충전 해제 확인 (`_SM_RUN_CHGOFF`)**: 충전 중이 아니라면(위 단계 통과), 확실한 시작을 위해 충전 해제 상태를 보장합니다.
5. **위치 확인 (`_SM_RUN_POSCHK`)**: 현재 위치를 모를 경우 위치 찾기 수행.
**시퀀스 단계 (Step-by-Step)**
1. **목표 설정**: `NodeMAP_RFID_Charger` 값을 읽어 충전기 노드를 목표(`TargetNode`)로 설정.
2. **충전기 위치 이동**:
* `UpdateMotionPositionForCharger` 함수를 사용해 충전기 앞단까지 이동.
* 제한시간(`ChargeSearchTime`) 초과 시 실패 처리 (`CHARGEOFF` 이동).
3. **정밀 검색 (Fine Search)**:
* 항상 **전진(Forward)**으로 미세 조정 이동하며 충전기를 검색합니다. (경로 예측 시 전진 진입하도록 설정됨).
* 음성 안내: "충전기를 검색합니다".
4. **마크 정지 (Mark Stop)**:
* AGV가 기동 중이면 `MarkStop` 명령 전송.
* 정지할 때까지 대기.
* 60초 내 정지 확인 안 되면 실패 처리.
5. **위치 확정**:
* 정지 후 3초 대기(센서 안정화).
* 현재 위치를 `CHARGE`로 설정.
6. **충전 명령**:
* `AGVCharge(true)` 호출하여 하드웨어에 충전 시작 명령 전송.
* `WAIT_CHARGEACK` 플래그 설정.
7. **ACK 확인**:
* AGV로부터 "CBT" 응답 대기.
* 응답 없을 시 재전송(최대 5회), 실패 시 취소 처리.
### 3.2 충전 확인 (`_SM_RUN_GOCHARGECHECK.cs`)
충전 명령 후 실제 배터리 충전이 시작되었는지 검증합니다.
* **조건**: `Battery_charging` 신호 및 `FLAG_CHARGEONA` 확인.
* **에러 처리**:
* `Charger_pos_error` (위치 에러) 또는 `Charger_run_error` 발생 시 취소.
* 30초 타임아웃: 충전이 시작되지 않으면 `CHARGEOFF`로 전환하여 프로세스 취소.
### 3.3 충전 해제 (`_SM_RUN_CHGOFF.cs`)
충전 상태를 안전하게 종료합니다.
* **로직**:
* `Battery_charging`이 true면 `AGVCharge(false)` 전송.
* 충전이 해제될 때까지 대기 (최대 1분).
* 안전하게 해제되면 `true` 반환.
## 4. 이동 제어 상세 (Movement Control)
* **핵심 함수**: `UpdateMotionPositionForCharger` (in `_Util.cs`)
* **특징**:
* 일반 이동과 달리 충전기 진입을 위한 전용 로직 사용.
* `MARK_SENSOR`를 확인하여 충전 위치 도달 여부 판단.
* **안전 로직 추가 (`CheckStopCondition`)**:
* **비상 정지**: `Emergency` 상태이고 로봇이 멈췄을 경우 `IDLE`로 전환 및 중단.
* **수동 충전 중**: `FLAG_CHARGEONM` 상태일 경우 이동 불가 처리.
## 5. 주요 설정 및 변수
* `PUB.setting.NodeMAP_RFID_Charger`: 충전기 위치의 RFID 태그 ID.
* `PUB.setting.chargerpos`: 충전 진입 방식 (0:전진, 2:후진, 1:QC위치).
* `PUB.setting.ChargeSearchTime`: 충전기 검색 제한 시간.

View File

@@ -1,63 +0,0 @@
# GOHOME 상태 머신 분석
## 개요
`GOHOME` 상태 머신(`_SM_RUN_GOHOME`)은 AGV를 현재 위치에서 미리 정의된 "Home" 노드로 이동시키는 역할을 합니다. 이 과정은 `_Util.cs``VirtualAGV.cs`에 정의된 공유 경로 탐색 및 이동 로직을 활용합니다.
## 로직 흐름
### 1. 초기화 및 안전 점검
- **하드웨어 확인:** `PUB.AGV.IsOpen`을 확인합니다. 연결이 끊겨 있으면 에러 상태로 설정합니다.
- **충전 해제:** `_SM_RUN_CHGOFF`를 호출하여 AGV가 물리적으로 충전기에 연결되어 있지 않은지 확인합니다.
- *잠재적 문제:* 충전 센서가 고착(stuck)된 경우, 이 단계에서 무한 대기할 수 있습니다.
- **Lidar 안전:** `PUB.AGV.system1.stop_by_front_detect`를 확인합니다. 장애물이 감지되면 제거될 때까지 실행을 일시 중지(false 반환)합니다.
### 2. 1단계: 목적지 설정
- **홈 위치 조회:** `PUB.setting.NodeMAP_RFID_Home`에서 홈 노드 ID를 가져옵니다.
- **유효성 검사:** 맵에서 홈 노드를 찾을 수 없는 경우, 에러를 기록하고 `READY` 상태로 초기화합니다.
- **타겟 할당:** `PUB._virtualAGV.TargetNode`를 홈 노드로 설정합니다.
### 3. 2단계: 이동 실행
- **위임:** `_Util.cs``UpdateMotionPositionForMark("_SM_RUN_GOHOME")`를 호출합니다.
- **경로 탐색:**
- 경로가 없거나 현재 경로가 유효하지 않은 경우, `_Util.cs``CurrentNode`에서 `TargetNode`까지의 새로운 경로를 계산합니다.
- **예측 및 제어:**
- `VirtualAGV.Predict()`가 현재 노드와 경로를 기반으로 다음 행동을 결정합니다.
- **정지 조건:** `Predict()``Stop`을 반환하는 경우:
- `IsPositionConfirmed`가 true인지 확인합니다.
- `CurrentNodeId``TargetNode.NodeId`와 일치하는지 확인합니다.
- 둘 다 true이면 `true`(도착)를 반환합니다.
- **이동 조건:** `Predict()`가 이동 명령을 반환하는 경우:
- 논리적 명령(좌/우/직진, 전진/후진, 속도)을 하드웨어 명령(`AGVMoveSet`)으로 변환합니다.
- **최적화:** 상태가 변경된 경우에만 명령을 전송합니다 (최근 `_Util.cs` 업데이트에 구현됨).
- AGV가 구동 중인지 확인합니다 (`AGVMoveRun`).
### 4. 3단계: 완료
- **알림:** "홈 검색 완료" 음성을 출력합니다.
- **로깅:** 데이터베이스에 기록을 추가합니다.
- **전환:** 시퀀스를 업데이트하여 `GOHOME` 상태를 사실상 종료합니다.
## 잠재적 문제 및 위험 요소
### 1. 충전 센서에 의한 무한 대기
- **위험:** 충전 신호가 활성화되어 있는 한 `_SM_RUN_CHGOFF` 호출은 계속 `false`를 반환합니다.
- **시나리오:** 센서가 고장 나거나 시스템이 모르는 사이에 AGV가 수동으로 충전기에 밀려 올라간 경우, 여기서 멈출 수 있습니다.
- **완화:** `_SM_RUN_CHGOFF` 확인에 타임아웃이나 수동 오버라이드 기능을 추가해야 합니다 (현재 스니펫에서는 보이지 않음).
### 2. 위치 상실 복구
- **위험:** `_SM_RUN_POSCHK`(`UpdateMotionPositionForMark`에서 호출됨)가 태그를 찾지 못하면, AGV가 무한히 기어갈(crawl) 수 있거나 멈출 수 있습니다.
- **메커니즘:** `_SM_RUN_POSCHK`는 태그를 찾기 위해 저속 전진 명령을 내립니다.
- **시나리오:** AGV가 경로를 벗어났거나 데드존에 있는 경우, Lidar가 감지하지 못하면 충돌하거나 배회할 수 있습니다.
### 3. 경로 재계산 루프 (확인됨: 안전함)
- **분석:** `_Util.cs` 코드를 확인한 결과, `CalcPath`가 실패하여 경로가 생성되지 않으면(`PathResult.result == null`) 즉시 `PUB.sm.SetNewRunStep(ERunStep.READY)`를 호출합니다.
- **결론:** 경로 계산 실패 시 상태 머신이 `READY` 상태로 전환되므로, 무한 루프나 반복적인 재계산 시도는 발생하지 않습니다. 안전하게 정지합니다.
### 4. 통신 폭주 (해결됨)
- **이전 위험:** 동일한 이동 명령의 지속적인 전송.
- **해결:** 최근 `_Util.cs` 업데이트에서 `PUB.AGV.data`를 새 명령과 비교하여 변경 시에만 전송하도록 수정되었습니다.
## 권장 사항
1. **충전 해제 타임아웃:** 센서 고장 시 무한 대기를 방지하기 위해 `_SM_RUN_CHGOFF` 확인에 타임아웃을 추가하십시오.
2. **경로 실패 처리:** `CalcPath`가 null/empty를 반환하는지 명시적으로 확인하고, 무한 재시도 대신 에러와 함께 `GOHOME` 시퀀스를 중단하십시오.
3. **도착 확인:** `_Util.cs`의 "도착" 확인이 "거의 도착" 시나리오(예: 태그 약간 앞에서 정지)에 대해 견고한지 확인하십시오. 현재 로직은 `CurrentNodeId` 업데이트에 의존하며, 이는 양호해 보입니다.

View File

@@ -0,0 +1,66 @@
# Home Move Sequence 분석 (Home Move Sequence Analysis)
## 1. 개요 (Overview)
이 문서는 AGV의 "Home Move" 기능에 대한 분석 결과를 기술합니다. UI 트리거부터 내부 상태 머신(State Machine) 로직 및 이동 제어 흐름을 포함합니다.
## 2. UI 트리거 (UI Trigger)
* **버튼 컨트롤**: `btHome` (in `fMain.Designer.cs`)
* **이벤트 핸들러**: `brHome_Click` (in `fMain.cs`)
* **동작**:
* 사용자가 "홈" 버튼 클릭 시 실행됩니다.
* **이미 Home 이동 중인 경우**: "홈 이동을 취소 할까요?" 팝업 → 확인 시 `IDLE` 상태로 전환 및 정지 (`AGVMoveStop`).
* **대기 상태인 경우**: "홈 이동을 실행 할까요?" 팝업 → 확인 시 `GOHOME` RunStep으로 설정 및 `RUN` 상태 시작.
* **조건**: 자동 충전 중이거나 수동 충전 모드가 아니어야 함 (`PUB.CheckManualChargeMode`).
## 3. 상태 머신 로직 (State Machine Logic)
* **핵심 파일**: `Project\StateMachine\Step\_SM_RUN_GOHOME.cs`
* **함수**: `_SM_RUN_GOHOME(bool isFirst, TimeSpan stepTime)`
* **상태 흐름**:
### 초기 검사
1. **하드웨어 연결**: `PUB.AGV.IsOpen` 체크. 연결 끊김 시 에러 처리.
2. **충전 상태**: `_SM_RUN_CHGOFF`를 호출하여 충전기가 분리되었는지 확인.
3. **장애물 감지**: `PUB.AGV.system1.stop_by_front_detect` (Lidar 감지)
* 감지 시 "전방에 물체가 감지되었습니다" 음성 출력.
* **주의**: 이 단계에서 명시적인 소프트웨어 정지 명령(`AGVMoveStop`)은 코드상에 보이지 않으며, 함수가 `false`를 리턴하여 이동 로직 진입을 막습니다. (하드웨어 자체 정지 기능에 의존하는 것으로 추정됨)
### 시퀀스 단계 (Sequence Steps)
상태 머신은 `PUB.sm.RunStepSeq`를 통해 단계별로 진행됩니다.
**단계 1: 이동 준비**
* 음성 안내: "홈으로 이동합니다"
* **목표 설정**: `PUB.setting.NodeMAP_RFID_Home` 값을 읽어 목표 노드(`TargetNode`)로 설정.
* 에러 처리: 홈 위치 설정이 없는 경우 에러 로그 기록 후 `READY` 상태로 복귀.
**단계 2: 이동 (Moving)**
* **이동 함수**: `UpdateMotionPositionForMark("funcName")` 호출 (in `_Util.cs`)
* 이 함수가 `true`를 반환할 때까지 반복 호출됨.
* 이동 완료 시 `AGVMoveStop` 호출 후 다음 단계로 진행.
**단계 3: 완료 (Completion)**
* 음성 안내: "홈 검색 완료"
* 로그 기록: 홈 위치 도착 기록.
* 상태 종료: `true` 반환 → 메인 루프에서 `READY` 상태로 전환.
## 4. 이동 제어 상세 (Movement Control Details)
* **핵심 함수**: `UpdateMotionPositionForMark` (in `Project\StateMachine\Step\_Util.cs`)
* **경로 계산**: 현재 위치와 목표 노드 간의 경로(`CalcPath`)를 계산.
* **주행 예측 (Predict)**: `PUB._virtualAGV.Predict()`를 사용하여 다음 동작(직진, 회전, 정지 등)을 결정.
* **제어 명령**:
* `Predict` 결과에 따라 모터(`Forward/Backward`), 분기(Magnetic Guide), 속도를 설정.
* 변경된 명령이 있을 경우에만 `PUB.AGV.AGVMoveSet`으로 하드웨어에 전송.
* AGV가 정지 상태라면 `PUB.AGV.AGVMoveRun`으로 구동 시작.
* **정지 조건**:
* `Predict``Stop` 명령을 반환하고,
* 위치가 확정(`IsPositionConfirmed`)되었으며,
* 현재 노드가 목표 노드와 일치하고,
* 실제 AGV 하드웨어가 정지(`agv_run == false`)한 경우.
## 5. 잠재적 이슈 (Potential Issues)
1. **장애물 감지 시 정지 처리**:
* `_SM_RUN_GOHOME`에서 장애물 감지 시(`stop_by_front_detect`), 음성 출력 후 `return false`만 수행합니다.
* 이전에 이동 명령이 전송된 상태라면, 소프트웨어에서 명시적으로 `AGVMoveStop`을 보내지 않으므로 AGV가 계속 이동하려 할 수 있습니다. (단, 하드웨어 센서가 직접 모터를 차단하도록 설계되었을 가능성이 높음)
* **권장**: 안전을 위해 장애물 감지 루틴 내에서도 명시적으로 `AGVMoveStop`을 호출하는 것을 검토할 필요가 있습니다.
2. **경로 재생성 시 멈춤**:
* `UpdateMotionPositionForMark`에서 경로를 재생성해야 할 경우, `AGVMoveStop("경로재생성")`을 호출하여 일시 정지 후 경로를 다시 계산합니다. 이는 안전한 동작입니다.

View File

@@ -0,0 +1,101 @@
# Predict() 함수 분석 보고서
## 1. 개요 (Overview)
`AGVNavigationCore.Models.VirtualAGV` 클래스의 `Predict()` 함수는 AGV의 현재 상태(위치, RFID 감지, 경로 등)를 기반으로 **다음 행동(이동, 정지, 속도 등)**을 결정하는 핵심 의사결정 함수입니다. 시스템은 이 함수를 주기적으로 호출하여 AGV가 경로를 이탈하지 않고 목적지까지 안전하게 이동하도록 제어합니다.
## 2. 논리 흐름 (Logic Flow)
`Predict()` 함수의 결정 로직은 다음 순서로 진행됩니다.
```mermaid
graph TD
A[Start Predict] --> B{위치 확정 여부<br/>(RFID 2개 이상)}
B -- No --> C[UnknownPosition<br/>(전진/저속/탐색)]
B -- Yes --> D{현재 경로(Path) 존재?}
D -- No --> E[NoPath<br/>(정지)]
D -- Yes --> F{목적지 도착 임박?<br/>(이전 노드 모두 통과)}
F -- Yes --> G{현재 노드 == 최종 노드?}
G -- Yes --> H{최종 노드 완료(IsPass)?}
H -- Yes --> I[Complete<br/>(정지/완료)]
H -- No --> J[MarkStop<br/>(정지/마크센서대기)]
G -- No --> K[Logic Continue]
F -- No --> K
K --> L{경로 이탈 확인<br/>(현재노드가 경로에 존재?)}
L -- No --> M[PathOut<br/>(정지/재탐색요청)]
L -- Yes --> N[GetCommandFromPath<br/>(다음 노드 이동 명령)]
```
### 단계별 상세 분석
1. **위치 미확정 (Position Check)**
* **조건**: `!_isPositionConfirmed` (감지된 RFID 개수 < 2)
* **행동**: `UnknownPosition` 리턴.
* **명령**: `Forward` (전진), `SpeedLevel.L` (저속).
* **목적**: RFID를 추가로 찾아 위치를 확정하기 위해 천천히 이동.
2. **경로 없음 (Path Check)**
* **조건**: `_currentPath`가 null이거나 비어있음.
* **행동**: `NoPath` 리턴.
* **명령**: `Stop`.
3. **목적지 도착 확인 (Goal Check)**
* **조건**: 경로상의 마지막 노드(`lastNode`) 이전의 모든 노드가 `IsPass == true` 상태인지 확인.
* **분기**:
* **최종 완료**: `_currentNode`가 마지막 노드이고, `IsPass == true`로 설정됨 -> `Complete` 리턴.
* **도착 직전(MarkStop)**: `_currentNode`가 마지막 노드이지만, 아직 `IsPass == false`임 -> `MarkStop` 리턴. 이는 물리적인 정지(Mark Sensor 감지)를 기다리는 상태임.
4. **경로 이탈 확인 (Deviation Check)**
* **로직**: 현재 경로(`DetailedPath`) 중에서 `_currentNode`와 ID가 일치하고 아직 지나가지 않은(`IsPass == false`) 노드를 찾음.
* **결과**: 찾지 못하면 `PathOut` 리턴 (경로 이탈로 간주하여 정지).
5. **이동 명령 생성 (Command Generation)**
* **함수**: `GetCommandFromPath(CurrentNodeId)` 호출.
* **로직**: 현재 노드에 정의된 이동 지침(모터 방향, 마그넷 분기, 속도 등)을 가져옴.
* **명령**: 해당 지침대로 `Normal` 명령 리턴.
---
## 3. 주요 메커니즘 및 의존성
### 3.1. 자동 패스 (Auto-Pass) 메커니즘
AGV가 중간 노드의 RFID를 놓치고 다음 노드로 건너뛰었을 때, 로직이 깨지지 않게 하는 중요한 안전장치입니다.
* **위치**: `SetPosition()` 함수 내 (Line 583~593).
* **동작**: AGV가 새로운 노드(B)에 도착했다고 보고되면, 경로상에서 B보다 앞선 모든 노드(A)의 `IsPass` 속성을 강제로 `true`로 변경합니다.
* **효과**: `Predict`의 "목적지 도착 확인" 로직(`All Previous Passed`)이 올바르게 작동하도록 보장합니다.
### 3.2. MarkStop 및 완료 처리
* AGV가 마지막 노드 RFID를 읽으면 `Predict``MarkStop`을 리턴합니다 (아직 `IsPass`는 false).
* `_Util.cs` 등 제어부는 이를 보고 감속/정지 준비를 합니다.
* AGV가 물리적으로 마크 센서에 정지하면 `_AGV.cs`에서 `SetCurrentNodeMarkStop()`을 호출합니다.
* 이때 비로소 마지막 노드의 `IsPass``true`가 됩니다.
* 다음 `Predict` 호출 시 `Complete`가 리턴되어 시퀀스가 종료됩니다.
---
## 4. 식별된 문제점 및 제안 사항 (Issues & Suggestions)
### 4.1. "Skipped Node" 시나리오의 잠재적 위험
* **현상**: `SetPosition`은 RFID 수신 시 호출되어 이전 노드들을 Auto-Pass 처리합니다. 하지만 만약 AGV가 경로를 이탈하여 엉뚱한 노드로 갔는데, 우연히 그 노드가 경로의 훨씬 뒤쪽 노드라면?
* **위험**: 중간의 모든 공정을 건너뛰고 바로 목적지 근처로 인식할 수 있습니다.
* **제안**: `SetPosition`에서 건너뛰는 노드의 개수가 너무 많거나(예: 3개 이상), 거리가 물리적으로 불가능할 정도로 멀다면 에러(`PathJump`)를 발생시키는 안전장치 추가 고려.
### 4.2. 하드코딩된 상수 (Magic Numbers)
* **코드**: `DetectedRfidCount >= 2`, `BatteryLevel < 20.0f` 등.
* **제안**: 이러한 값들을 `const` 또는 설정 파일(`PUB.setting`)로 관리하여 유지보수성을 높여야 합니다.
### 4.3. `GetCommandFromPath`의 Null 처리 불일치
* **현상**: `Predict`에서는 `PathOut`을 먼저 체크하지만, 기저에 있는 `GetCommandFromPath`에도 중복된 Null 체크/`NoTarget` 리턴 로직이 있음.
* **제안**: 로직의 일관성을 위해 검증 로직을 통일하거나, `Predict`에서 확실히 걸러낸 후 `GetCommandFromPath`는 데이터를 가져오는 역할만 하도록 단순화.
### 4.4. `Predict`의 "현재 노드 기준" 명령 생성
* **현상**: `GetCommandFromPath``CurrentNodeId`를 인자로 받습니다. 즉, "현재 노드에서 수행해야 할 행동"을 반환합니다.
* **의문**: 만약 AGV가 노드와 노드 사이(Edge)에 있을 때, `CurrentNodeId`는 "지난 노드"입니다. 이때 "지난 노드"의 명령을 계속 수행하는 것이 맞는지(일반적으로는 맞음, Go Forward 등) 확인 필요.
* **확인**: 현재 로직은 맞습니다. 다음 RFID를 만날 때까지 이전 명령을 유지합니다.
---
## 5. 결론 (Conclusion)
`Predict()` 함수는 `SetPosition`(위치 업데이트 및 Auto-Pass) 및 `MarkStop` 처리 로직과 유기적으로 결합되어 있어 단독으로만 보면 이해하기 어려울 수 있습니다. 현재 로직은 **RFID 누락(Skip)에 대한 복구 능력**을 갖추고 있으며, **물리적 정지(MarkStop)와 논리적 완료(Complete)를 구분**하여 정밀한 제어를 가능하게 설계되어 있습니다.
다만, 매직 넘버 사용과 일부 중복된 검증 로직은 리팩토링을 통해 개선할 여지가 있습니다.