638 lines
23 KiB
C#
638 lines
23 KiB
C#
//#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<string> GetAlarm() => null;
|
|
|
|
// 네이명 변경 대상 : GetHWPosLimit
|
|
public override Range2<double> GetPosLimit() {
|
|
if (_baseConfig is null) MessageUtil.ShowErrorToast("_baseConfig is null: PLC must be activated before call it", Log);
|
|
return _baseConfig?.PosLimit ?? new Range2<double>(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
|
|
//===============================================================
|
|
}
|
|
} |