using Aurora.Api; using Hermes; using System; using System.Diagnostics; using System.Runtime.InteropServices; using System.Threading; using static AuroraPLC.PLCBase; namespace AuroraLaserScanner { public class ScannerSimul : ScannerObj { static new readonly Logger Log = new Logger("Scanner"); // For storage of each queried data set, the user program must provide (at an address specified by Ptr) an area of PC main memory equal to 528520 bytes. [MarshalAs(UnmanagedType.LPArray, SizeConst = 528520)] uint[] m_coordinationTransformPtr = new uint[528520]; //set_trigger 관련 추가, get_waveform 에서 얻어올 포인터 추가, 최대 크기로 설정 (2^20 = 1,048,576) [MarshalAs(UnmanagedType.LPArray, SizeConst = 1048576)] int[] m_getWaveformPtr = new int[1048576]; private bool _ModulationMode; private ScannerConfig _config; // ----------------------------- // MOTF Simulation state // ----------------------------- public Func PLCReadBit; public Func PLCReadWord; public Func PLCReadDWord; public Func PLCGetAbsDeg; private const int PLC_CURRENT_POS_1_ADDR = 4; public Stopwatch sw = Stopwatch.StartNew(); // encoder가 반환할 값 public volatile int _simEncXCount = 0; //랩핑 각도(0~360 public volatile int _simEncYCount = 0; //누적 각도 public volatile bool LastWaitMissedRange = false; private const int RotaryJigCount = 16; private const double JigPerMsec = 300; private const double DegreePerRev = 360.0; private const double OneCycleMsec = JigPerMsec * RotaryJigCount; // 누적각을 scanner 내부에서 유지 public double _angleAbsDeg = 0.0; private bool _listClosed = false; private bool _list1Busy = false; private bool _list2Busy = false; private eListNo _currentExecutingList = eListNo.NONE; private bool _autoChangeRequested = false; 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; // 리스트 실행 시간 private const double RecommendWindowDeg = 45.0; private const double RecommendUseRatio = 0.35; // 45도 중 실제 마킹이 차지하는 비율(가볍게) public void EquipmentSimulator() { PullEncoderFromPlcOnce(); UpdateListExecution(); } private int DegToCount(double deg) { double c = deg * _config.MarkingOnTheFlyEncoderResolutionRotate / DegreePerRev; return (int)System.Math.Round(c); } private int DegToCountAbs(double absDeg) { double c = absDeg * _config.MarkingOnTheFlyEncoderResolutionRotate / DegreePerRev; long lc = (long)System.Math.Round(c); return unchecked((int)lc); } private void UpdateListExecution() { if (_currentExecutingList == eListNo.NONE) return; long now = sw.ElapsedMilliseconds; // ── 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가 음수인 경우 방지 if (elapsed < 0) { _listStartMs = now; return; } if (elapsed >= doneTime) { // 현재 리스트 실행 완료 if (_currentExecutingList == eListNo.LIST1) _list1Busy = false; else _list2Busy = false; if (_autoChangeRequested) { // 반대 리스트 자동 실행 _currentExecutingList = (_currentExecutingList == eListNo.LIST1) ? eListNo.LIST2 : eListNo.LIST1; if (_currentExecutingList == eListNo.LIST1) { _list1Busy = true; _list2Busy = false; } else { _list2Busy = true; _list1Busy = false; } _autoChangeRequested = false; _encoderConditionMet = false; // 새 리스트의 엔코더 조건 초기화 bool newHasWait = (_currentExecutingList == eListNo.LIST1) ? _list1HasEncoderWait : _list2HasEncoderWait; if (!newHasWait) _listStartMs = now; // 엔코더 대기 없으면 마킹 타이머 즉시 시작 } else { _currentExecutingList = eListNo.NONE; } } } private int GetEffectiveListTimeMs() { int t = SimulatedListTimeMs; // 4800msec / 360 * 45 * ratio int recommendedListRunTime = (int)Math.Round((OneCycleMsec / DegreePerRev) * RecommendWindowDeg * RecommendUseRatio); // recommended가 너무 작거나 0이 되는 경우 방지 if (recommendedListRunTime < MinListTimeMs) recommendedListRunTime = MinListTimeMs; if (t < MinListTimeMs) t = MinListTimeMs; if (t > MaxListTimeMs) t = MaxListTimeMs; if (recommendedListRunTime > MaxListTimeMs) recommendedListRunTime = MaxListTimeMs; return t; } public void TrySyncAbsAngleToPlcNow() { _angleAbsDeg = PLCGetAbsDeg(); // mod 동기화도 같이 int modRaw = PLCReadDWord(PLC_CURRENT_POS_1_ADDR, ePLCDevice.W, false).data; double modDeg = (modRaw / 100_000.0) % DegreePerRev; if (modDeg < 0) modDeg += DegreePerRev; LastWaitMissedRange = false; } public void ResetSim() { _angleAbsDeg = 0.0; _simEncXCount = 0; _simEncYCount = 0; StopExecution(); // 리스트 상태 리셋 LastWaitMissedRange = false; } public override uint Open() { IsOpenUnit = true; return NO_ERROR; } public override void Close() { if (IsOpenUnit == true) { IsOpenUnit = false; } } public override int Initialize(ScannerConfig config) { CommandPosX = 0; CommandPosY = 0; CommandPosZ = 0; _ModulationMode = config.Modulation; _config = config; return E1.OK; } public override bool GetModulationMode() { return _ModulationMode; } public override void JumpAbs(double x, double y, eFunctionType type = eFunctionType.COMMAND) { CommandPosX = x; CommandPosY = y; } public override void JumpAbs3D(double x, double y, double z, eFunctionType type = eFunctionType.COMMAND) { CommandPosX = x; CommandPosY = y; CommandPosZ = z; } public override void JumpRel(double x, double y, eFunctionType type = eFunctionType.COMMAND) { CommandPosX += x; CommandPosY += y; } public override void JumpRel3D(double x, double y, double z, eFunctionType type = eFunctionType.COMMAND) { CommandPosX += x; CommandPosY += y; CommandPosZ += z; } public override void MarkAbs(double x, double y) { CommandPosX = x; CommandPosY = y; } public override void MarkRel(double x, double y) { CommandPosX += x; CommandPosY += y; } public override void ArcAbs(double x, double y, double angle) { CommandPosX = x; CommandPosY = y; } public override void ArcRel(double x, double y, double angle) { CommandPosX += x; CommandPosY += y; } public override void JumpAbsZ(double z, eFunctionType type = eFunctionType.COMMAND) { CommandPosZ = z; } public override void JumpRelZ(double z, eFunctionType type = eFunctionType.COMMAND) { CommandPosZ += z; } public override void GetXYPos(out double x, out double y) { x = CommandPosX; y = CommandPosY; } public override void GetZPos(out double z) { z = CommandPosZ; } public override void WriteDA(uint portNo, float value/*0~100%*/, eFunctionType type = eFunctionType.COMMAND) { } public override void GetEncoder(out double encoderX, out double encoderY, bool rotateMode = false) { int valueX = _simEncXCount; int valueY = _simEncYCount; // 너가 쓰는 변환식 그대로 encoderX = (valueX * DegreePerRev) / _config.MarkingOnTheFlyEncoderResolutionRotate; encoderY = _angleAbsDeg; if (encoderY > -0.5 && encoderY < 0) encoderY = 0; // 시뮬 안정화 return; } public override uint GetMarkingInfo() { //return RTC5Wrap.n_get_marking_info(m_deviceNo); return 0; } public override uint GetStartStopInfo() { //return RTC5Wrap.n_get_startstop_info(m_deviceNo); return 0; } public override void StoreEncoder(uint pos) { //RTC5Wrap.n_store_encoder(m_deviceNo, pos); } public override void ReadEncoder(out double encoder0_0, out double encoder1_0, out double encoder0_1, out double encoder1_1, bool rotateMode = false) { // simulation 할때 값 반영 검토 필요 encoder0_0 = 0; encoder1_0 = 0; encoder0_1 = 0; encoder1_1 = 0; } public override void WaitForEncoderX(double position) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderY(double position) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRotateX(double angle) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRotateY(double angle) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRotateXMode(double angle, eEncoderWaitThroughMode mode) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRotateYMode(double angle, eEncoderWaitThroughMode mode) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderXMode(double position, eEncoderWaitThroughMode mode) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderYMode(double position, eEncoderWaitThroughMode mode) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderMode(double position, uint encoderNo, eEncoderWaitThroughMode mode) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRange(double minX, double maxX, double minY, double maxY) { // simulation 할때 값 반영 검토 필요 } public override void WaitForEncoderRotateRange(double minX, double maxX, double minY, double maxY) { // 실제 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() { // 방어: delegate 미설정이면 아무 것도 못함 if (PLCReadDWord == null || _config == null || _config.MarkingOnTheFlyEncoderResolutionRotate == 0) return; // 1) 랩핑각 (0~360) int modRaw = PLCReadDWord(PLC_CURRENT_POS_1_ADDR, ePLCDevice.W, true).data; double angleModDeg = (modRaw / 100_000.0) % 360.0; if (angleModDeg < 0) angleModDeg += 360.0; // 2) 누적각 (계속 증가) //int absRaw = PLCReadDWord(PLC_CURRENT_POS_ABS_ADDR, ePLCDevice.W, true).data; //double angleAbsDeg = absRaw / 100_000.0; if (PLCGetAbsDeg != null) { // InitFly2D 시점 기준 상대각 유지 (실제 RTC 엔코더 리셋 동작과 동일) _angleAbsDeg = PLCGetAbsDeg() - _flyResetAbsDeg; } // 3) count로 변환 (n_get_encoder가 반환한다고 가정하는 값) _simEncXCount = DegToCount(angleModDeg); _simEncYCount = DegToCountAbs(_angleAbsDeg); } public override void SetFlyX(double encoderResolution) { // simulation 할때 값 반영 검토 필요 } public override void SetFlyX(double encoderResolution, double diameter) { // simulation 할때 값 반영 검토 필요 } public override void SetFlyY(double encoderResolution) { // simulation 할때 값 반영 검토 필요 } public override void SetRotateCenter(double posX, double posY, eFunctionType type = eFunctionType.COMMAND) { // simulation 할때 값 반영 검토 필요 } public override void SetFlyRotate(double encoderResolution) { // simulation 할때 값 반영 검토 필요 } public override void SetFlyRotatePos(double encoderResolution) { // simulation 할때 값 반영 검토 필요 } public override void ActivateFlyXY(double encoderResolutionX, double encoderResolutionY) { // simulation 할때 값 반영 검토 필요 } public override void ActivateFly1Axis(int axis, int mode, double scale, int offset) { } public override void Fly_Return(double x, double y) { // simulation 할때 값 반영 검토 필요 } public override void ParkPosition(double posX, double posY, eParkType type = eParkType.VirtualImage) { // simulation 할때 값 반영 검토 필요 } public override void ParkReturn(double posX, double posY, eParkType type = eParkType.VirtualImage) { // simulation 할때 값 반영 검토 필요 } public override void InitFly2D(int offsetX = 0, int offsetY = 0) { // 실제 RTC InitFly2D는 펌웨어 MOTF 엔코더 카운터를 0으로 리셋. // 시뮬에서는 현재 PLC 누적각을 기준점으로 저장하여 이후 _angleAbsDeg가 // 0부터 시작하는 상대각이 되도록 함 → sectionStartAngle 계산과 좌표계 일치. _flyResetAbsDeg = (PLCGetAbsDeg != null) ? PLCGetAbsDeg() : 0.0; _angleAbsDeg = 0.0; } public override void SimulateEncoder(uint EncoderNo) { // simulation 할때 값 반영 검토 필요 } public override void SetControlMode(uint mode) { // simulation 할때 값 반영 검토 필요 } public override void SetLaesrDelay(int laserOnDelay, uint laserOffDelay) { // simulation 할때 값 반영 검토 필요 } public override void SetScannerDelay(uint jumpDelay, uint markDelay, uint arcDelay) { // simulation 할때 값 반영 검토 필요 } public override void SetJumpSpeedCtrl(double speed) { // simulation 할때 값 반영 검토 필요 } public override void SetJumpSpeed(double speed) { // simulation 할때 값 반영 검토 필요 } public override void SetMarkSpeed(double speed) { // simulation 할때 값 반영 검토 필요 } public override void SetStartList(eListNo no) { // simulation 할때 값 반영 검토 필요 } public override uint LoadList(eListNo no) { bool busy = (no == eListNo.LIST1) ? _list1Busy : _list2Busy; 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) { if (!_listClosed) Log.W("ExecuteList called without SetEndList"); _listClosed = false; // 실행 시작하면서 다시 열림 상태로 _currentExecutingList = no; _encoderConditionMet = false; // 엔코더 조건부터 평가 if (no == eListNo.LIST1) { _list1Busy = true; _list2Busy = false; } else { _list2Busy = true; _list1Busy = false; } bool hasWait = (no == eListNo.LIST1) ? _list1HasEncoderWait : _list2HasEncoderWait; if (!hasWait) _listStartMs = sw.ElapsedMilliseconds; // 엔코더 대기 없으면 마킹 타이머 즉시 시작 } public override void StartLoop() { RTC5Wrap.n_start_loop(m_deviceNo); } public override void QuitLoop() { // simulation 할때 값 반영 검토 필요 } public override void AutoChange() { if (_currentExecutingList == eListNo.NONE) return; _autoChangeRequested = true; } public override bool GetBusyStatus() { return _list1Busy || _list2Busy || _currentExecutingList != eListNo.NONE; } public override void StopExecution() { _list1Busy = false; _list2Busy = false; _currentExecutingList = eListNo.NONE; _autoChangeRequested = false; _listStartMs = 0; _listClosed = false; _recordingList = eListNo.NONE; _list1HasEncoderWait = false; _list2HasEncoderWait = false; _encoderConditionMet = false; _flyResetAbsDeg = 0.0; _angleAbsDeg = 0.0; _simEncXCount = 0; _simEncYCount = 0; } public override uint GetRTCVersion() { // simulation 할때 값 반영 검토 필요 return 0; } public override uint GetDLLVersion() { // simulation 할때 값 반영 검토 필요 return 0; } public override void SetLaserTiming(uint periodHz, uint laser1PulseWidthUs, uint laser2PulseWidthUs) //단위는 Khz, us임 { // simulation 할때 값 반영 검토 필요 } public override void SetStandby(uint periodHz, uint laserPulseWidthUs) //단위는 Khz, us임 { // simulation 할때 값 반영 검토 필요 } public override void SetLaserMode(eLaserMode mode) { // simulation 할때 값 반영 검토 필요 } public override void SetLaserControl(uint ctrl) { // simulation 할때 값 반영 검토 필요 } public override void SetFirstPulseKiller(uint width) //단위는 us->bit 로 변경해야 함 { // simulation 할때 값 반영 검토 필요 } public override void LaserSignalOn() { // simulation 할때 값 반영 검토 필요 } public override void LaserSignalOff() { // simulation 할때 값 반영 검토 필요 } /// /// 지정한 시간 시간만큼 Laser Active 신호를 켜주는 리스트 함수 /// /// 시간[ms] public override void LaserOnList(double interval) { // simulation 할때 값 반영 검토 필요 } /// /// Laser Active가 Off된 상태에서 지정한 시간 시간만큼 delay를 추가하는 함수 /// /// 시간[ms] public override void LongDelay(uint delay) { // simulation 할때 값 반영 검토 필요 } public override void SetAngle(double angle/*degree*/, uint atOnce, eFunctionType type = eFunctionType.COMMAND) { // simulation 할때 값 반영 검토 필요 } public override void SetOffset(double offsetX, double offsetY, uint atOnce, eFunctionType type = eFunctionType.COMMAND) { // simulation 할때 값 반영 검토 필요 } public override void DisableLaser() { // simulation 할때 값 반영 검토 필요 } public override void EnableLaser() { // simulation 할때 값 반영 검토 필요 } public override void ParaLaserOnPulsesList(double period, uint pulses, float powerValue) { // simulation 할때 값 반영 검토 필요 } //Ramping 기능 public override void ParaMarkAbs(double x, double y, float power) { CommandPosX = x; CommandPosY = y; // simulation 할때 값 반영 검토 필요 } public override void ParaMarkRel(double x, double y, float power) { CommandPosX += x; CommandPosY += y; // simulation 할때 값 반영 검토 필요 } public override void ParaJumpAbs(double x, double y, float power) { CommandPosX = x; CommandPosY = y; // simulation 할때 값 반영 검토 필요 } public override void ParaJumpRel(double x, double y, float power) { CommandPosX += x; CommandPosY += y; // simulation 할때 값 반영 검토 필요 } public override void SetVectorControl(uint portNo, uint value) { // simulation 할때 값 반영 검토 필요 } //측정 기능 public override void StopTrigger() { // simulation 할때 값 반영 검토 필요 } public override void SetTrigger(uint period/*㎲*/, eRecordSignal signal1, eRecordSignal signal2, eRecordSignal signal3, eRecordSignal signal4) { // simulation 할때 값 반영 검토 필요 } public override void SetTrigger(uint period/*㎲*/, eRecordSignal signal1, eRecordSignal signal2) { // simulation 할때 값 반영 검토 필요 } public override void Measurement_status(out uint busy, out uint pos) { // simulation 할때 값 반영 검토 필요 busy = 0; pos = 0; } public override uint ReadStatus() { // simulation 할때 값 반영 검토 필요 return 0; } public override void GetWaveForm(uint channel, uint number, out int[] data) { data = new int[number]; // simulation 할때 값 반영 검토 필요 } //안오산-170224-Wobbel모드 추가(V1.7.0) //set_wobbel_mode public override void SetWobbelMode(bool wobbelEnable, double transversal, double longitudinal, double frequency, int shape) { // simulation 할때 값 반영 검토 필요 } public override void SetWobbelDirection(int dX, int dY) { // simulation 할때 값 반영 검토 필요 } public override void SetWobbelVector(double tansversal, double longitudinal, uint period, float power) { // simulation 할때 값 반영 검토 필요 } public override void ConfigLaserSignal(eLaserSignal pin2, eLaserSignal pin1, eLaserSignal pin9) { // simulation 할때 값 반영 검토 필요 } public override void SelectCorrectionTable(int no) { // simulation 할때 값 반영 검토 필요 } public override void SetEthernetConfigLinkLoss(eEthConfigLinkLossMode mode) { } public override int CheckEthernetConnection(out uint errorCode) { errorCode = 0; return 0; } #region 리스트 실행 시간 측정 /// /// Delayed Short List Command /// Stores the current value of the RTC6 timer and resets it to zero. /// public override void SaveAndRestartTimer() { // simulation 할때 값 반영 검토 필요 } /// /// Ctrl Command /// Returns the RTC5 timer value (without resetting it to zero). It has been stored during the most recent call of save_and_restart_timer. /// /// RTC5 timer value in seconds. As a 64-bit IEEE floating point value. public override double GetTime() { // simulation 할때 값 반영 검토 필요 return 0; } #endregion // 기존 추상함수 아닌 3개 항목 추상함수로 추가 //ExecuteUpdateParameterList는 Initialize에 직접구현할수밖에.. config의 항목 쓰려면.. public override uint LoadProgramFile(string fileName) { // simulation 할때 값 반영 검토 필요 return 0; } public override uint LoadCorrectionFile(string fileName, uint correctionNo, bool use3DMode) { // simulation 할때 값 반영 검토 필요 return 0; } public static int CutValue(int value, int min, int max) { if (value > max) { return max; } else if (value < min) { return min; } else { return value; } } public override void SetLaserPinOutList(uint pins) { } public override void SetPortDefault(uint port, uint value) { } public override void SetLaserPinOut(uint pins) { } public override uint GetLastError() { return 0; } public override uint GetListSpace() { return 0; } public override void SetConfigList(uint mem1, uint mem2) { } public override void GetListPointer(out eListNo list_no, out uint list_pos) { list_no = eListNo.NONE; if (_list1Busy) list_no = eListNo.LIST1; else if (_list2Busy) list_no = eListNo.LIST2; list_pos = 0; } } }