Scanner Simulation Add
This commit is contained in:
parent
632e9b0a5e
commit
c9c8860356
@ -53,6 +53,22 @@ namespace AuroraLaserScanner
|
||||
|
||||
private long _listStartMs = 0; // ← 이게 executionStartTime 역할
|
||||
|
||||
// 리스트 기록 대상 추적 (LoadList 성공 시 설정, SetEndList 시 초기화)
|
||||
private eListNo _recordingList = eListNo.NONE;
|
||||
|
||||
// 리스트별 WaitForEncoderRotateRange 조건 저장 (실제 RTC 펌웨어 리스트 명령 역할)
|
||||
private bool _list1HasEncoderWait;
|
||||
private double _list1EncoderMinY, _list1EncoderMaxY;
|
||||
private bool _list2HasEncoderWait;
|
||||
private double _list2EncoderMinY, _list2EncoderMaxY;
|
||||
|
||||
// 현재 실행 중인 리스트의 엔코더 조건 충족 여부
|
||||
private bool _encoderConditionMet;
|
||||
|
||||
// InitFly2D() 호출 시점의 PLC 누적각 (실제 RTC 엔코더 리셋과 동일한 역할)
|
||||
// _angleAbsDeg = PLCGetAbsDeg() - _flyResetAbsDeg 로 항상 InitFly2D 이후 상대각을 유지
|
||||
private double _flyResetAbsDeg = 0.0;
|
||||
|
||||
private const int MinListTimeMs = 30; // 너무 빠른 완료 방지
|
||||
private const int MaxListTimeMs = 800; // 너무 느린 완료 방지
|
||||
private int SimulatedListTimeMs = 250; // 리스트 실행 시간
|
||||
@ -85,8 +101,25 @@ namespace AuroraLaserScanner
|
||||
return;
|
||||
|
||||
long now = sw.ElapsedMilliseconds;
|
||||
long elapsed = now - _listStartMs;
|
||||
|
||||
// ── Phase 1: 엔코더 조건 대기 ───────────────────────────────────────
|
||||
// 실제 RTC 펌웨어에서 WaitForEncoderRotateRange 명령이 리스트 실행 중
|
||||
// 엔코더가 구간에 진입할 때까지 대기하는 동작을 모사.
|
||||
bool hasWait = (_currentExecutingList == eListNo.LIST1) ? _list1HasEncoderWait : _list2HasEncoderWait;
|
||||
if (hasWait && !_encoderConditionMet) {
|
||||
double minY = (_currentExecutingList == eListNo.LIST1) ? _list1EncoderMinY : _list2EncoderMinY;
|
||||
double maxY = (_currentExecutingList == eListNo.LIST1) ? _list1EncoderMaxY : _list2EncoderMaxY;
|
||||
// _angleAbsDeg는 PullEncoderFromPlcOnce에서 InitFly2D 기준 상대각으로 갱신됨
|
||||
double absDeg = _angleAbsDeg;
|
||||
if (absDeg >= minY && absDeg <= maxY) {
|
||||
_encoderConditionMet = true;
|
||||
_listStartMs = now; // 조건 충족 시점부터 마킹 타이머 시작
|
||||
}
|
||||
return; // 조건 미충족이면 busy 유지, 충족 직후도 이번 주기는 return
|
||||
}
|
||||
|
||||
// ── Phase 2: 마킹 시간 경과 후 리스트 완료 ─────────────────────────
|
||||
long elapsed = now - _listStartMs;
|
||||
int doneTime = GetEffectiveListTimeMs();
|
||||
|
||||
// 최소 현실 가드: elapsed가 음수인 경우 방지
|
||||
@ -115,8 +148,12 @@ namespace AuroraLaserScanner
|
||||
_list1Busy = false;
|
||||
}
|
||||
|
||||
_listStartMs = now;
|
||||
_autoChangeRequested = false;
|
||||
_encoderConditionMet = false; // 새 리스트의 엔코더 조건 초기화
|
||||
|
||||
bool newHasWait = (_currentExecutingList == eListNo.LIST1) ? _list1HasEncoderWait : _list2HasEncoderWait;
|
||||
if (!newHasWait)
|
||||
_listStartMs = now; // 엔코더 대기 없으면 마킹 타이머 즉시 시작
|
||||
} else {
|
||||
_currentExecutingList = eListNo.NONE;
|
||||
}
|
||||
@ -344,37 +381,18 @@ namespace AuroraLaserScanner
|
||||
|
||||
public override void WaitForEncoderRotateRange(double minX, double maxX, double minY, double maxY)
|
||||
{
|
||||
LastWaitMissedRange = false;
|
||||
|
||||
long t0 = sw.ElapsedMilliseconds;
|
||||
|
||||
while (true) {
|
||||
// 블로킹 대기 중에도 PLC에서 최신값을 직접 당겨온다
|
||||
PullEncoderFromPlcOnce();
|
||||
|
||||
// GetEncoder는 _simEncYCount를 deg로 바꿔줄 것
|
||||
GetEncoder(out _, out double curY, true);
|
||||
|
||||
// 정상 진입
|
||||
if (curY >= minY && curY <= maxY)
|
||||
return;
|
||||
|
||||
// 최소 현실 가드: 구간을 지나쳤으면 TIMEOUT까지 끌지 말고 즉시 종료
|
||||
// (실장에서도 "놓치면" 의미 없고 다음 로직이 꼬임)
|
||||
if (curY > maxY) {
|
||||
LastWaitMissedRange = true;
|
||||
Log.W($"[SIM] WaitForEncoderRotateRange MISSED. curY={curY:F3}, range=[{minY:F3}..{maxY:F3}]");
|
||||
return;
|
||||
}
|
||||
|
||||
// 최후 안전장치
|
||||
if (sw.ElapsedMilliseconds - t0 > 5000) {
|
||||
LastWaitMissedRange = true;
|
||||
Log.W($"[SIM] WaitForEncoderRotateRange TIMEOUT. curY={curY:F3}, range=[{minY:F3}..{maxY:F3}]");
|
||||
return;
|
||||
}
|
||||
|
||||
Thread.Sleep(1);
|
||||
// 실제 RTC 펌웨어와 동일하게: PC 스레드를 블로킹하지 않고 리스트 명령으로 조건 저장.
|
||||
// EquipmentSimulator → UpdateListExecution 에서 PLCGetAbsDeg()가 [minY, maxY] 구간
|
||||
// 진입을 확인한 후 마킹 타이머를 시작하므로, 그 전까지 리스트는 busy 상태를 유지.
|
||||
// LoadList 폴링(CheckLoadComplete)이 busy 해소를 기다리는 방식으로 타이밍 동기화.
|
||||
if (_recordingList == eListNo.LIST1) {
|
||||
_list1HasEncoderWait = true;
|
||||
_list1EncoderMinY = minY;
|
||||
_list1EncoderMaxY = maxY;
|
||||
} else if (_recordingList == eListNo.LIST2) {
|
||||
_list2HasEncoderWait = true;
|
||||
_list2EncoderMinY = minY;
|
||||
_list2EncoderMaxY = maxY;
|
||||
}
|
||||
}
|
||||
private void PullEncoderFromPlcOnce()
|
||||
@ -393,7 +411,8 @@ namespace AuroraLaserScanner
|
||||
//int absRaw = PLCReadDWord(PLC_CURRENT_POS_ABS_ADDR, ePLCDevice.W, true).data;
|
||||
//double angleAbsDeg = absRaw / 100_000.0;
|
||||
if (PLCGetAbsDeg != null) {
|
||||
_angleAbsDeg = PLCGetAbsDeg();
|
||||
// InitFly2D 시점 기준 상대각 유지 (실제 RTC 엔코더 리셋 동작과 동일)
|
||||
_angleAbsDeg = PLCGetAbsDeg() - _flyResetAbsDeg;
|
||||
}
|
||||
|
||||
// 3) count로 변환 (n_get_encoder가 반환한다고 가정하는 값)
|
||||
@ -455,7 +474,11 @@ namespace AuroraLaserScanner
|
||||
|
||||
public override void InitFly2D(int offsetX = 0, int offsetY = 0)
|
||||
{
|
||||
// simulation 할때 값 반영 검토 필요
|
||||
// 실제 RTC InitFly2D는 펌웨어 MOTF 엔코더 카운터를 0으로 리셋.
|
||||
// 시뮬에서는 현재 PLC 누적각을 기준점으로 저장하여 이후 _angleAbsDeg가
|
||||
// 0부터 시작하는 상대각이 되도록 함 → sectionStartAngle 계산과 좌표계 일치.
|
||||
_flyResetAbsDeg = (PLCGetAbsDeg != null) ? PLCGetAbsDeg() : 0.0;
|
||||
_angleAbsDeg = 0.0;
|
||||
}
|
||||
public override void SimulateEncoder(uint EncoderNo)
|
||||
{
|
||||
@ -498,11 +521,20 @@ namespace AuroraLaserScanner
|
||||
if (busy)
|
||||
return 0; // 실행 중 → Load 불가
|
||||
|
||||
// 기록 대상 리스트 확정 및 엔코더 대기 조건 초기화
|
||||
// (WaitForEncoderRotateRange가 이번 사이클에서 호출될 때 여기 저장된다)
|
||||
if (_recordingList != no) {
|
||||
_recordingList = no;
|
||||
if (no == eListNo.LIST1) _list1HasEncoderWait = false;
|
||||
else _list2HasEncoderWait = false;
|
||||
}
|
||||
|
||||
return (uint)no; // Load 가능
|
||||
}
|
||||
public override void SetEndList()
|
||||
{
|
||||
_listClosed = true;
|
||||
_recordingList = eListNo.NONE; // 기록 완료
|
||||
}
|
||||
public override void ExecuteList(eListNo no)
|
||||
{
|
||||
@ -511,6 +543,7 @@ namespace AuroraLaserScanner
|
||||
_listClosed = false; // 실행 시작하면서 다시 열림 상태로
|
||||
|
||||
_currentExecutingList = no;
|
||||
_encoderConditionMet = false; // 엔코더 조건부터 평가
|
||||
|
||||
if (no == eListNo.LIST1) {
|
||||
_list1Busy = true;
|
||||
@ -520,7 +553,9 @@ namespace AuroraLaserScanner
|
||||
_list1Busy = false;
|
||||
}
|
||||
|
||||
_listStartMs = sw.ElapsedMilliseconds;
|
||||
bool hasWait = (no == eListNo.LIST1) ? _list1HasEncoderWait : _list2HasEncoderWait;
|
||||
if (!hasWait)
|
||||
_listStartMs = sw.ElapsedMilliseconds; // 엔코더 대기 없으면 마킹 타이머 즉시 시작
|
||||
}
|
||||
public override void StartLoop()
|
||||
{
|
||||
@ -550,6 +585,12 @@ namespace AuroraLaserScanner
|
||||
_listStartMs = 0;
|
||||
_listClosed = false;
|
||||
|
||||
_recordingList = eListNo.NONE;
|
||||
_list1HasEncoderWait = false;
|
||||
_list2HasEncoderWait = false;
|
||||
_encoderConditionMet = false;
|
||||
|
||||
_flyResetAbsDeg = 0.0;
|
||||
_angleAbsDeg = 0.0;
|
||||
|
||||
_simEncXCount = 0;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user