//#define LUKAS_D_PLCMOTOR using Aurora.Api; using Aurora.Api.Devices; using Aurora.View; using AuroraDeviceCommon.Utils; using AuroraMotor; using Hermes; using System; using System.Collections.Generic; using System.Threading; using static AuroraDeviceCommon.Utils.Utils; namespace AuroraPLC { // 외부에서는 부모 클래스인 PLCMotorBase 를 사용 // 따라서, 필요한 필드/메서드를 모두 갖고 있어야 함. // 인터페이스 비슷한 역할... // 모션 관련 모든 메모리 주소를 모두 갖고 있다고 가정... public abstract class PLCMotorBase : AuroraMotorBase { static Logger Log = new Logger("PLCMotorBase"); #region PLC 관련 // 자체적으로 독립적인 운영 및 순환참조를 막기위해서 // PLCBase 의 인터페이스를 소유 // 의존성 주입(DI)를 위해서 인터페이스를 인자로 보내는 건 번거로움 (콜백 제거의 이점이 없는듯..) protected IPLCBase _plc; #endregion //public int MotorIdx { get; set; } //public string MotorId { get; set; } //public bool IsSimul { get; private set; } public double LoopPeriod { get; set; } = 0.01; // 근사값 [s] (PLC 업데이트 스레드 Sleep 시간) private object lockPLCMotor = new object(); public PLCMotorBaseConfig _baseConfig; public bool IsMemMapConfigured { get; set; } = false; #region 모션 HW 세팅 파라메터 // 세팅 // HW 세팅값 double _maxVel; // [mm/s] double _maxAccel; // [mm/s^2] //public double _nLimit { get; private set; } //public double _pLimit { get; private set; } #endregion #region 모션 SW 세팅 파라메터 //public double _speedVal; // [mm/s] //double _speedPerc; // [0 ~ 1] double _jogSpeedPerc; // [0 ~ 1] // scale parameter public double _scalePos { get; protected set; } = 1; public double _scaleVel { get; protected set; } = 1; // 지울까...?? public double ExtSpeedRatio { get; set; } // [0.01 ~ 1] #endregion #region 궤적 관련 // motionTimeStart DateTime mMoveStartTime; // 제어 출력 //public double _targetPos, _targetVel; // 내부버퍼 사용 //public double _targetVel; //public double _actualPos; // 내부버퍼 사용 double jogTargetVel; // 조깅 모션 생성용 double _trajOutPos, _trajOutVel, _trajOutAcc; double _startPos, _moveDistance; #endregion #region 모션 상태 정보 public bool IsMoveMotion { get; private set; } // 이건 시뮬레이션 용도임... bool isJogging; public bool IsEnableSwitchPressed { get; private set; } // -1: Negative, +1: Positive int motorDir = 1; #endregion // 0 : Plan, 1: Move, 2: Wait, 3: Hold //int motionStatus = 0; #region PLC-specfiic public int _addrReqMove, _addrReqMoveAck, _addrCmdErr; public int _addrIsMovable, _addrIsMoving; // isMoving 은 Busy 랑 동의어일까? public int _addrJogP, _addrJogN; public int _addrReadyJogP, _addrReadyJogN; // +,- 조그 가능상태 주소 public int _addrStopMove, _addrStopMoveAck; public int _addrTargetPos, _addrActualPos; public int _addrTargetSPD, _addrActualVel; public int _addrTargetJogSPD, _addrActualJogSPD; // 타겟 위치를 넘기고 Move 요청을 위한 동기화 코드 ushort latchForUpdatePointer; ulong latchForUpdateCycle; #endregion #region 생성자 public PLCMotorBase(string motorId, bool isSimul, IPLCBase plc) : base(motorId) { IsSimul = isSimul; _plc = plc; //LoopPeriod = loopPeriod; // 0.01 [s] = 약 100 Hz // 기본 초기화 ExtSpeedRatio = 1.0; // PLC 모션 축의 최대 속도가 어떻게 되는거지 ??? // [mm/s] _maxVel = 500; // 일단 Jog Popup 기준 (추후 확인후 수정 필요) // 가감속 : 0.3 [s] 기준으로 세팅 (manual jog 와 move 를 구분할까 말까..???) // [mm/s^2] _maxAccel = _maxVel / 0.3; //_scalePos = scalePos; //_scaleVel = scaleVel; } public PLCMotorBase(string motorId, bool isSimul, IPLCBase plc, int motorIdx) : base(motorId, motorIdx) { IsSimul = isSimul; _plc = plc; //LoopPeriod = loopPeriod; // 0.01 [s] = 약 100 Hz // 기본 초기화 ExtSpeedRatio = 1.0; // PLC 모션 축의 최대 속도가 어떻게 되는거지 ??? // [mm/s] _maxVel = 500; // 일단 Jog Popup 기준 (추후 확인후 수정 필요) // 가감속 : 0.3 [s] 기준으로 세팅 (manual jog 와 move 를 구분할까 말까..???) // [mm/s^2] _maxAccel = _maxVel / 0.3; //_scalePos = scalePos; //_scaleVel = scaleVel; } #endregion // PLC 모션의 기본으로 상정 (메모리 번지가 없으면 그때 수정하자) // Aurora 고정부에 맞추기 위해서 이름 변경 : GetActualPos => GetCurrentPosition public override double GetActualPos() => _plc.ReadDWord(_addrActualPos).data / _scalePos; public void SetActualPos(double actPos) => _plc.WriteDWord(_addrActualPos, (int)(actPos * _scalePos), isOutput: false); // For Simul // 내부 변수는 남기는게 좋을까? 테스트해보고 결정... void SetTargetPos(double tarPos) => _plc.WriteDWord(_addrTargetPos, (int)(tarPos * _scalePos)); public override double GetTargetPos() => _plc.ReadDWord(_addrTargetPos, isInput: false).data / _scalePos; // 속도는 virtual로 // 예외 검사는 외부에서... public override int SetTargetSPD(double tarSPD) => _plc.WriteDWord(_addrTargetSPD, (int)(tarSPD * _scaleVel)); // vel public override double GetTargetSPD() => _plc.ReadDWord(_addrTargetSPD, isInput: false).data / _scaleVel; // vel // Aurora PLC 와 실제 PLC 사이에는 통신에 의한 딜레이가 존재함.. // Aurora 의 모션 요청과 PLC 의 모션 중을 어떻게 판별할까..? public override bool IsMoving() => _plc.ReadBit(_addrIsMoving).data; public void SetIsMoving(bool isOn) => _plc.WriteBit(_addrIsMoving, isOn, isOutput: false); // For Simul public bool GetAck() => _plc.ReadBit(_addrReqMoveAck).data; public void SetAck(bool isOn) => _plc.WriteBit(_addrReqMoveAck, isOn, isOutput: false); // For Simul public int SetMoveReq(bool isOn) => _plc.WriteBit(_addrReqMove, isOn); public bool GetStopAck() => _plc.ReadBit(_addrStopMoveAck).data; public void SetStopAck(bool isOn) => _plc.WriteBit(_addrStopMoveAck, isOn, isOutput: false); // For Simul public int SetStopReq(bool isOn) => _plc.WriteBit(_addrStopMove, isOn); public bool IsJogOn() { return IsJogPOn() && IsJogNOn(); } public bool IsJogPOn() => _plc.ReadBit(_addrReadyJogP).data; public void SetJogPOn(bool isOn) { if (IsSimul) { _plc.WriteBit(_addrReadyJogP, isOn, isOutput: false); } } public bool IsJogNOn() => _plc.ReadBit(_addrReadyJogN).data; public void SetJogNOn(bool isOn) { if (IsSimul) { _plc.WriteBit(_addrReadyJogN, isOn, isOutput: false); } } #region 모션축 상태 모니터링 public virtual bool IsMovable() => _plc.ReadBit(_addrIsMovable).data; public virtual void SetMovable(bool isOn) { if (IsSimul) { _plc.WriteBit(_addrIsMovable, isOn, isOutput: false); } } public override bool IsTargetPosLimit() { if (_baseConfig.IsPosLimited == false) { return false; } var tarPos = GetTargetPos(); var posLimit = GetPosLimit(); if (tarPos < posLimit.Min || tarPos > posLimit.Max) { //Log.E($"{DeviceId}: NLimit/Target/PLimit: {posLimit.Min:F2}/{tarPos:F2}/{posLimit.Max:F2}"); return true; } else { return false; } } private int CheckPosLimit(in double tempTargetPos, ref double axisTargetPos) { if (_baseConfig.IsPosLimited == false) { axisTargetPos = tempTargetPos; return 0; } var posLimit = GetPosLimit(); if (tempTargetPos < posLimit.Min) { axisTargetPos = posLimit.Min; return -1; } else if (tempTargetPos > posLimit.Max) { axisTargetPos = posLimit.Max; return 1; } else { axisTargetPos = tempTargetPos; return 0; } } #endregion //============================================== #region 조깅 모션 public override int SetJogSPD(eJogSpeed jogSPD) { Log.V($"TargetJogSPD : {jogSPD.ToString()}"); switch (jogSPD) { case eJogSpeed.L: _jogSpeedPerc = 0.2; break; case eJogSpeed.M: _jogSpeedPerc = 0.6; break; case eJogSpeed.H: _jogSpeedPerc = 1.0; break; default: break; } // Machine PLC : 속도는 1 ~ 3 범위 // Low : 1, Mid : 2, High : 3 _plc.WriteWord(_addrTargetJogSPD, (short)jogSPD); return 0; } public double GetJogSPD() { return _maxVel * _jogSpeedPerc; } public override int Jog(int direction, double speedVal) { // 속도 세팅 필요 : Low / Mid / High #warning [LUKAS] Jog 에 속도는 따로 주도록 수정해달라고 할까? return Jog(direction); } public override int Jog(int direction) { // 동기화 문제가 있을 수 있으므로, 방어적으로 동기화 로직을 넣자 // WO 완료시점에서의 updatePointer 가 중요함 latchForUpdatePointer = _plc.LatchForUpdatePointer; latchForUpdateCycle = _plc.LatchForUpdateCycle; // 조그 타깃 속도에 대한 동기화...(Move 에 비해서 중요도가 높지는 않을것 같은데..) motorDir = direction; if (IsSimul) { IsMoveMotion = false; isJogging = true; SetIsMoving(true); IsEnableSwitchPressed = true; } // 동기화 처리 if (_plc.WaitWriteWordUpdate(latchForUpdatePointer, latchForUpdateCycle, out _) == false) { Log.W(WriteLogTag("WriteWord timed out (Need to debug)")); } // 메모리 주소때문에 virtual 로 선언 if (direction > 0) { _plc.WriteBit(_addrJogP, Bit.On); } else { _plc.WriteBit(_addrJogN, Bit.On); } return 0; } void JogCalIncrement() { // 타겟 조깅 속도값 (goal) 계산 double velGoal; double tempAcc; velGoal = 0.0; // 조깅키 눌림 if (IsEnableSwitchPressed) { var speedVal = GetJogSPD(); // 목표 속도 : 세팅 속도값을 직접 사용 velGoal = (motorDir > 0) ? speedVal : -speedVal; } // if 인칭 모드 는 인칭 or 궤적 쪽에서 처리하자... // else // 타겟속도는 아래에서만 업데이트됨.. tempAcc = Math.Abs(velGoal - jogTargetVel) / LoopPeriod; if (tempAcc <= _maxAccel * ExtSpeedRatio) { // 등속 구간(정지 포함) 혹은 다음 사이클에서 등속구간(정지 포함) 진입 가능 jogTargetVel = velGoal; } else if (jogTargetVel > velGoal) { // 감속 구간 jogTargetVel -= (_maxAccel * ExtSpeedRatio * LoopPeriod); } else { // 가속 구간 jogTargetVel += (_maxAccel * ExtSpeedRatio * LoopPeriod); } #if LUKAS_D_PLCMOTOR //Debug.WriteLine($"JogCalIncrement: {IsEnableSwitchPressed}, {motorDir}, {speedVal}, {_maxAccel}, {LoopPeriod}"); //Debug.WriteLine($"JogCalIncrement: {jogTargetVel}, {tempAcc}"); #endif } void JogGeneration() { double tempTargetPos, curTargetPos, newTargetPos, axisTargetPos; tempTargetPos = curTargetPos = newTargetPos = axisTargetPos = 0; // 현재값 및 위치 증분값 업데이트 curTargetPos = _trajOutPos; tempTargetPos = curTargetPos + (jogTargetVel * LoopPeriod); // 보통 조깅은 단축임...시뮬 모드에만 쓸거니, 적당하게 (Aurora는 실제 모션 제어는 전용 모션제어기에서 하므로!!!) int checkLimit = CheckPosLimit(in tempTargetPos, ref axisTargetPos); // 위치 리밋에 걸려있어도 리밋 안으로 들어올 수 있게 허용하는 로직 (Only for 시뮬 모드) if (checkLimit != 0) { // 위치 리밋 case var posLimit = GetPosLimit(); if (tempTargetPos > axisTargetPos) { if (curTargetPos > tempTargetPos) { newTargetPos = tempTargetPos; } else if (curTargetPos < posLimit.Max) { newTargetPos = axisTargetPos; } else { newTargetPos = curTargetPos; } } else if (tempTargetPos < axisTargetPos) { if (curTargetPos < tempTargetPos) { newTargetPos = tempTargetPos; } else if (curTargetPos > posLimit.Min) { newTargetPos = axisTargetPos; } else { newTargetPos = curTargetPos; } } else { newTargetPos = axisTargetPos; } } else { // No Limit newTargetPos = tempTargetPos; } // 최종 조깅 궤적 할당 double newTargetVel = (newTargetPos - _trajOutPos) / LoopPeriod; // 속도 _trajOutAcc = (newTargetVel - _trajOutVel) / LoopPeriod; // 가속도 _trajOutVel = newTargetVel; _trajOutPos = newTargetPos; // 조깅 정지 여부 판단 기준 if (IsEnableSwitchPressed == false) { // 정지 지령 상태 var l_movingAxisCnt = 0; if (IsEqual(_trajOutVel, 0.0) != true || IsEqual(_trajOutAcc, 0.0) != true) { // 타겟 속도가 0 이 아닌 경우 or 타겟 가속도가 0 이 아닌 경우, ++l_movingAxisCnt; } if (l_movingAxisCnt == 0) { // 만약 0 이면, 정지 상태임 // 조깅 동작이 완료되었음을 의미.. isJogging = false; SetIsMoving(false); #if LUKAS_D_PLCMOTOR //Debug.WriteLine($"JogGeneration: IsEnableSWPress: {IsEnableSwitchPressed}, isJogging: {isJogging}"); #endif } } } #endregion #region 일반 모션 public override int Move() { //24.02.01 choieigun Move Command 이동 시, Limit 확인 필요 if (IsTargetPosLimit()) { MessageUtil.ShowErrorToast("Pos Limit Violation", Log); return -1; } // 동기화 처리 if (_plc.WaitWriteWordUpdate(latchForUpdatePointer, latchForUpdateCycle, out _) == false) { Log.W(WriteLogTag("WriteWord timed out (Need to debug)")); } if (IsSimul) { var actPos = GetActualPos(); _startPos = actPos; _moveDistance = GetTargetPos() - actPos; motorDir = _moveDistance > 0 ? 1 : -1; mMoveStartTime = DateTime.Now; IsMoveMotion = true; SetIsMoving(true); } // 모션 기동 요청 return SetMoveReq(Bit.On); } #endregion #region 모션 정지 public override int StopJog() { if (IsSimul) { // 조깅 정지 trigger IsEnableSwitchPressed = false; // 기존 방식은 감속 정지 없이 그대로 멈춤...;; IsMoveMotion = false; // 모션쪽은 궤적 알고리즘 적용 검토... SetIsMoving(false); } // 그냥 전부 다 Off 하자... _plc.WriteBit(_addrJogP, Bit.Off); _plc.WriteBit(_addrJogN, Bit.Off); return 0; } public override int Stop() { if (IsSimul) { // 조깅 정지 trigger IsEnableSwitchPressed = false; // 기존 방식은 감속 정지 없이 그대로 멈춤...;; IsMoveMotion = false; // 모션쪽은 궤적 알고리즘 적용 검토... SetIsMoving(false); } return SetStopReq(Bit.On); } #endregion #region 모션 세팅 // LUKAS_COMMENT: 2번째 인자가 디폴트 매개변수를 갖고 있음. 메모리맵을 접근하는 프로퍼티랑 이름이 같은데, 프로퍼티 이름을 바꿀까? public override int SetTargetPos(double targetPos, TargetPosType targetPosType = TargetPosType.ABS) { if (targetPosType == TargetPosType.ABS) { var posLimit = GetPosLimit(); if (targetPos < posLimit.Min) { //_plc.AlarmService.SetAlarm((int)ePLCErrCode.Motion_TargetPosNLimit); MessageUtil.ShowErrorToast($"{DeviceId}: {ePLCErrCode.Motion_TargetPosNLimit.ToString()}: Target/NLimit: {targetPos:F2}/{posLimit.Min:F2}", Log); return (int)ePLCErrCode.Motion_TargetPosNLimit; } else if (targetPos > posLimit.Max) { //_plc.AlarmService.SetAlarm((int)ePLCErrCode.Motion_TargetPosPLimit); MessageUtil.ShowErrorToast($"{DeviceId}: {ePLCErrCode.Motion_TargetPosPLimit.ToString()}: Target/PLimit: {targetPos:F2}/{posLimit.Max:F2}", Log); return (int)ePLCErrCode.Motion_TargetPosPLimit; } SetTargetPos(targetPos); // 동기화 문제가 있을 수 있으므로, 방어적으로 동기화 로직을 넣자 // WO 완료시점에서의 updatePointer 가 중요함 latchForUpdatePointer = _plc.LatchForUpdatePointer; latchForUpdateCycle = _plc.LatchForUpdateCycle; return E1.OK; } else { return SetTargetPos(GetActualPos() + targetPos, TargetPosType.ABS); } } // Aurora 의 경우, HW = SW 동일... 한 가지밖에 없으므로... public void SetHWPosLimit(double NLimit, double PLimit) { if (Math.Abs(NLimit) > _baseConfig.PosLimitThreshold && Math.Abs(PLimit) > _baseConfig.PosLimitThreshold) { // 무한대로 처리 _baseConfig.IsPosLimited = false; // IsPosLimited 에서 자동 업데이트됨... //_baseConfig.PosLimit.Min = Double.NegativeInfinity; //_baseConfig.PosLimit.Max = Double.PositiveInfinity; } else { // 무한대 flag 해제 _baseConfig.IsPosLimited = true; _baseConfig.PosLimit.Min = NLimit; _baseConfig.PosLimit.Max = PLimit; } } #endregion #region Aurora 고정부 인터페이스 public override int ClearAlarm() => 0; public override IList GetAlarm() => null; // 네이명 변경 대상 : GetHWPosLimit public override Range2 GetPosLimit() { if (_baseConfig is null) MessageUtil.ShowErrorToast("_baseConfig is null: PLC must be activated before call it", Log); return _baseConfig?.PosLimit ?? new Range2(0, 0); } #endregion #region 모션 업데이트 public void UpdateMotion() { Monitor.Enter(lockPLCMotor); if (isJogging) { // 조깅 모션 생성 or 정지 모션 생성 // 내부에서 IsEnableSwitchPressed flag 처리함 // 증분값 계산... // 최대 속도, 최대 가속도 정보 JogCalIncrement(); // 움직인다고 가정해보자... 굳이 안 움직이는 경우를 구분해야 할까..? //Debug.WriteLine($"jogTargetVel: {jogTargetVel}"); JogGeneration(); /* position += distance; if (!mIsHomeCompleted && diff > 10) { mAlarmList.Add( (MotorDirection > 0) ? "POT: (+) over travel" : "NOT: (-) over travel"); mIsJogging = false; } */ // 여기는 시뮬레이션 모드에서만 실행됨... // 조그의 경우는 타깃 위치도 업데이트해보자... SetTargetPos(_trajOutPos); #if LUKAS_D_PLCMOTOR Debug.WriteLine($"Jogging: Pos: {_trajOutPos:F3}"); #endif } else if (IsMoveMotion) { // 여긴 기존방식을 적용하자 : 가감속이 없음.. var elaspedTime = (DateTime.Now - mMoveStartTime).TotalSeconds; // 경과된 시간 // 러프한 근사: 등속도로 가정한 총 이동거리 double distance = motorDir * GetTargetSPD() * elaspedTime; // actualPos 가 필요한가..? 필요없으면 지우자.. double position = IsMoving() ? _startPos : GetActualPos(); if (Math.Abs(_moveDistance) > Math.Abs(distance)) { // 이동해야 할 거리 > 총 이동한 거리 position += distance; } else { // 이동해야 할 거리 <= 총 이동한 거리 (이동할 위치에 도달함) position += _moveDistance; IsMoveMotion = false; SetIsMoving(false); #if LUKAS_D_PLCMOTOR Debug.WriteLine($"MoveMotion is complete: Pos: {position}"); #endif } _trajOutPos = position; } // 시뮬레이션 : Plant 부분 // Plant state // 현재 상태 = 제어 출력 SetActualPos(_trajOutPos); //========================================= Monitor.Exit(lockPLCMotor); } #endregion //=============================================================== } }