using Aitex.Core.RT.DataCenter; using Aitex.Core.RT.Device; using Aitex.Core.RT.Event; using Aitex.Core.RT.Log; using Aitex.Core.RT.OperationCenter; using Aitex.Core.Util; using MECF.Framework.Common.Device.LinMot; using CyberX8_Core; using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Threading; using System.Threading.Tasks; using Aitex.Core.RT.Routine; using MECF.Framework.Common.ToolLayout; using System.Diagnostics; using Aitex.Core.RT.SCCore; namespace CyberX8_RT.Devices.LinMot { public class LinMotAxis : BaseDevice, IDevice { #region 常量 /// /// 控制字 /// private const string STATUS_WORD = "StatusWord"; private const string HOME = "Home"; private const string SWITCH_ON = "SwitchOn"; private const string DIRECTION = "Direction"; private const string POSITION = "Position"; private const string SCAN_COUNT = "ScanCount"; /// /// 下一指令判定条件与_startPosition的距离 /// private const double _nextInterval = 2; #endregion #region 内部变量 /// /// Home状态 /// private bool _isHomed; /// /// SwitchOn状态 /// private bool _isSwitchOn; /// /// Disabled状态 /// private bool _isDisabled; /// /// 错误状态 /// private bool _isError; /// /// 致命错误状态 /// private bool _isFatalError; /// /// 运动状态 /// private bool _isMotorOn; /// /// 是否到达目标 /// private bool _inTagertPosition = false; /// /// 操作当前状态 /// private RState _status; /// /// 当前操作指令 /// private LinMotOperation _currentOperation = LinMotOperation.None; /// /// 状态字 /// private short _statusWord; /// /// LinMotId /// private byte _linmotId; /// /// 错误代码 /// private short _errorCode; /// /// Reset Routine /// private LinMotResetRoutine _resetRoutine; /// /// Stop Motor Routine /// private LinMotStopMotorRoutine _stopMotorRoutine; /// /// Start Curve /// private LinMotStartContinueCurveRoutine _startContinueCurveRoutine; /// /// Start VAI /// private LinMotStartVAIPositionRoutine _startVAIPositionRoutine; /// /// Curve速度 /// private int _curverSpeed = 100; /// /// Curver指定的Id /// private byte _curveId = 1; /// /// 指令发送时间(用于空闲时发送读取状态指令) /// private DateTime _sendTime = DateTime.Now; /// /// 运动时间(用于判定运动指令发送LinMot并没有执行上报超时) /// private DateTime _motorTime = DateTime.Now; /// /// 设备参数对象 /// private LinMotDevice _linMotDeviceParam; /// /// 开始时的方向 /// private string _startDirection = ""; /// /// 当前GAI GO To Position方向 /// private string _direction = ""; /// /// 当前位置 /// private double _currentPosition =0; /// /// 当前位置(类型为整形) /// private int _currentIntPosition = 0; /// 用于prewet页面UI的位置数据 /// private double _currentPrewetUIPostion = 0; /// /// /// 判定方向位置 /// private double _judgePosition = 0; /// /// GAI运动前的位置 /// private double _startPosition = 0; /// /// 状态字时间 /// private DateTime _statusWordDateTime=DateTime.Now; /// /// linmot stop watch /// private Stopwatch _linmotStopWatch = new Stopwatch(); /// /// 状态字上升沿信号 /// private R_TRIG _statusTrigger = new R_TRIG(); #endregion #region 属性 /// /// 操作当前状态 /// public RState Status { get { return _status; } } /// /// Home /// public bool IsHomed { get { return IsConnectd && _isHomed; } } /// /// SwitchOn状态 /// public bool IsSwitchOn { get { return IsConnectd && _isSwitchOn; } } /// /// 错误 /// public bool IsError { get { return _isError; } } /// /// Disabled状态 /// public bool IsDisabled { get { return IsConnectd&&_isDisabled; } } /// /// 运动状态 /// public bool IsMotorOn { get { if(_isMotorOn) { return true; } else { if (_linmotStopWatch.ElapsedMilliseconds <= 2000) { return true; } else { return false; } } } } /// /// 是否已经到达目标 /// public bool InTargetPosition { get { return _inTagertPosition; } } /// /// 当前位置 /// public double CurrentPosition { get { return _currentPosition; } } /// /// 方向 /// public string Direction { get { return _direction; } } /// /// 连接状态 /// public bool IsConnectd { get { return LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module.ToString()); } } /// /// 扫描次数 /// public int ScanCount { get { return _startVAIPositionRoutine.CurrentScan; } } /// /// 当前位置 /// public int CurrentIntPosition { get { return _currentIntPosition; } } /// /// 最后是否SwitchOff /// public bool LastSwitchOff { get; set; } = false; /// /// curve速度 /// public int CurveSpeed { get { return _curverSpeed; } } /// /// 错误代码 /// public short ErrorCode { get { return _errorCode; } } #endregion /// /// 构造函数 /// /// /// public LinMotAxis(string moduleName) : base(moduleName, moduleName, moduleName, moduleName) { InitialData(); InitializeOperation(); InitializeRoute(); _linmotStopWatch.Restart(); } /// /// 初始化Routine /// private void InitializeRoute() { _resetRoutine = new LinMotResetRoutine(Module,this); _stopMotorRoutine=new LinMotStopMotorRoutine(Module,this); _startContinueCurveRoutine=new LinMotStartContinueCurveRoutine(Module,this); _startVAIPositionRoutine=new LinMotStartVAIPositionRoutine(Module,this); } /// /// 初始化OP /// private void InitializeOperation() { OP.Subscribe($"{Module}.Reset",(cmd,args)=> { return ResetOperation(cmd, LastSwitchOff); }); OP.Subscribe($"{Module}.StartCurve", StartCurveOperation); OP.Subscribe($"{Module}.StartPosition", StartPosition); OP.Subscribe($"{Module}.Stop", StopOperation); OP.Subscribe($"{Module}.Abort", AbortOperation); OP.Subscribe($"{Module}.UpdateSpeedData",UpdateSpeedDataOperation); LinMotDeviceConfigManager.Instance.InitialDevice(Module.ToString()); LinMotDeviceConfigManager.Instance.SubscribeModuleVariable(Module, UpdateStatusWord); } /// /// 初始化数据 /// private void InitialData() { _linMotDeviceParam = LinMotDeviceConfigManager.Instance.GetLinMotDevice(Module); if (_linMotDeviceParam == null) { LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), "Get Address Id Error"); ; return; } if (byte.TryParse(_linMotDeviceParam.Address, out var tmpLinmotId)) { _linmotId = tmpLinmotId; } DATA.Subscribe($"{Module}.{STATUS_WORD}", () => IsConnectd?_statusWord:0,SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.{HOME}", () => IsHomed,SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.{SWITCH_ON}", () => IsSwitchOn, SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.{DIRECTION}", () => _direction,SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.{POSITION}", () => _currentPosition, SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.{SCAN_COUNT}",()=>ScanCount,SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.Speed", () => _curverSpeed, SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.IsConnectd", () => IsConnectd, SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.CurrentPrewetUIPostion", () => _currentPrewetUIPostion, SubscriptionAttribute.FLAG.IgnoreSaveDB); DATA.Subscribe($"{Module}.LinmotDeviceData", () => _linMotDeviceParam.LinMotDeviceData, SubscriptionAttribute.FLAG.IgnoreSaveDB); } /// /// 更新状态字 /// /// /// /// private void UpdateStatusWord(short statusWord,short errorCode,int position) { _statusWordDateTime=DateTime.Now; _statusTrigger.CLK = _statusWord != statusWord; if (_statusTrigger.Q) { LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"statusword is {statusWord}"); } _statusWord = statusWord; UpdateStatusWord(statusWord); short tmpErrorCode = errorCode; NotifyError(tmpErrorCode); //ContinueGAIGotoPosition(); double tmpPosition = (double)position / 10000; if (Math.Abs(tmpPosition - _currentPosition) >= 0.1) { _currentPosition = tmpPosition; bool enableLog = SC.GetValue("Log.EnableLinmotLog"); if (enableLog) { LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"motor status {_isMotorOn} position {_currentPosition}"); } } _currentIntPosition = position; if (_linMotDeviceParam.LinMotDeviceData != null && _linMotDeviceParam.LinMotDeviceData.TopPosition - _linMotDeviceParam.LinMotDeviceData.BottomPosition != 0) { double proportion = _linMotDeviceParam.LinMotDeviceData.TopPosition - _linMotDeviceParam.LinMotDeviceData.BottomPosition; _currentPrewetUIPostion = 150 - 90 * Math.Abs(position / proportion); //150是prewet页面底部位置,90是页面顶部和底部距离之差 } JudgeDirection(); } /// /// 更新StatusWord /// /// private void UpdateStatusWord(short statusWord) { bool isHomed = (statusWord & 0b0100000000000) >> 11 == 1;//11位为Home if (isHomed != _isHomed) { _isHomed = isHomed; } _isDisabled = (statusWord & 0b0100) >> 2 == 0; _isSwitchOn = (statusWord & 0x01) == 1; _isError = ((statusWord & 0b01000) >> 3 == 1) || ((statusWord & 0b01000000000000) >> 12 == 1); _isFatalError = ((statusWord & 0b01000000000000) >> 12 == 1); bool isMotorOn = ((statusWord & 0b010000000000000) >> 13 == 1);//13是否正在运动 if (isMotorOn != _isMotorOn) { _isMotorOn = isMotorOn; if (!_isMotorOn) { _linmotStopWatch.Restart(); } } _inTagertPosition = (statusWord & 0b010000000000) >> 10 == 1;//10位是否到达目标 } /// /// 通知错误 /// /// private void NotifyError(short tmpErrorCode) { if (tmpErrorCode != _errorCode) { if (tmpErrorCode != 0) { string errorMsg = LinMotErrorCodeManager.Instance.GetErrorCodeText(tmpErrorCode); string str = string.IsNullOrEmpty(errorMsg) ? tmpErrorCode.ToString() : errorMsg; if (!_isFatalError) { LOG.WriteLog(eEvent.ERR_LINMOT, Module, $"{str},please reset error"); } else { LOG.WriteLog(eEvent.ERR_LINMOT, Module, $"{str},meets fatal error,please reboot {Module}"); } _currentOperation = LinMotOperation.None; _status = RState.Failed; } _errorCode = tmpErrorCode; } } /// /// 判定方向 /// private void JudgeDirection() { if(_linMotDeviceParam!=null&&_linMotDeviceParam.LinMotDeviceData!=null&&_currentOperation==LinMotOperation.StartVAIGoToPosition) { double interval = _currentPosition - _judgePosition; if(Math.Abs(interval)>=1) { string tmpDirection = ""; if(interval>0) { tmpDirection = "down"; } else { tmpDirection = "up"; } if(tmpDirection!=_direction) { //_sendNextGAIPosition = false; _direction = tmpDirection; } _judgePosition = _currentPosition; } } } /// /// Reset /// /// /// /// public bool ResetOperation(string cmd, bool lastNeedSwitchoff) { if(_status==RState.Running) { LOG.WriteLog(eEvent.ERR_LINMOT,Module.ToString(), $"{Module} current execute {_currentOperation},cannot Reset"); return false; } if(!LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module)) { LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), $"device is not connected"); return false; } _currentOperation = LinMotOperation.Reset; _status= _resetRoutine.Start(lastNeedSwitchoff); _direction = ""; return _status==RState.Running; } /// /// 开始Curve /// /// /// /// public bool StartCurveOperation(string cmd, object[] args) { if (_status == RState.Running) { EV.PostAlarmLog(Module.ToString(), eEvent.INFO_LINMOT, $"{Module} current execute {_currentOperation},cannot curve"); return false; } _motorTime = DateTime.Now; object[] param= (object[])args[1]; int speed = (int)param[0]; return StartCurve(speed); } /// /// 开始Curve /// /// /// public bool StartCurve(int speed) { _currentOperation = LinMotOperation.StartCurve; _curverSpeed = speed; _status= _startContinueCurveRoutine.Start(_curverSpeed); LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"Start Curve With Speed {_curverSpeed}%"); return _status==RState.Running; } /// /// 开始Curve速度 /// /// /// public bool ChangeCurveSpeed(int speed) { _curverSpeed= speed; return WriteRamIntValue(0xCB, 0x14, speed * 100); } /// /// 开始移动(GAI Goto Position) /// /// /// /// public bool StartPosition(string cmd, object[] args) { if (_linMotDeviceParam == null || _linMotDeviceParam.LinMotDeviceData == null) { LOG.WriteLog(eEvent.ERR_LINMOT, Module.ToString(), $"{Module} config error"); return false; } if (_status == RState.Running) { LOG.WriteLog(eEvent.ERR_LINMOT,Module.ToString(), $"{Module} current execute {_currentOperation},cannot Start VAI Go To Position"); return false; } _currentOperation = LinMotOperation.StartVAIGoToPosition; if (_linMotDeviceParam.LinMotDeviceData.TopPosition < 0) { _direction = "up"; } else { _direction = "down"; } _startDirection = _direction; _judgePosition = _currentPosition; _startPosition = _currentPosition; int totalScan = (int)args[0] ; _status= _startVAIPositionRoutine.Start(totalScan, _linMotDeviceParam.LinMotDeviceData); return _status==RState.Running; } /// /// SwitchOff /// /// /// /// private bool AbortOperation(string cmd, object[] args) { if(_currentOperation==LinMotOperation.StartVAIGoToPosition) { _startVAIPositionRoutine.Abort(); } _currentOperation = LinMotOperation.None; _status = RState.End; _direction = ""; LOG.WriteLog(eEvent.INFO_LINMOT, Module, "switch off"); return SendOperation(LinMotOperation.SwitchOff); } /// /// 更新linmot速度参数 /// /// /// /// private bool UpdateSpeedDataOperation(string cmd, object[] args) { string name = args[0].ToString(); string direction = args[1].ToString(); double maxSpeed = (double)args[2]; int maxAcc = (int)args[3]; int maxDec = (int)args[4]; LinMotDeviceConfigManager.Instance.UpdateSpeedData(name, direction, maxSpeed,maxAcc,maxDec); LOG.WriteLog(eEvent.INFO_LINMOT, Module, $"{Module} Speed related paramater was updated"); return true; } /// /// 发送Curve指令 /// private void SendCurveCommand() { _motorTime = DateTime.Now; _sendTime = DateTime.Now; LinMotDeviceConfigManager.Instance.SendCurveOperation(Module.ToString(), _linmotId, _curveId,(ushort)_curverSpeed); } /// /// 发送GAI GoToPosition /// /// /// /// /// public bool SendVAIGoToPosition(int position,int speed,int accel,int decel) { _motorTime = DateTime.Now; _sendTime = DateTime.Now; return LinMotDeviceConfigManager.Instance.SendVAIGotoPosition(Module.ToString(), _linmotId, position, speed,accel, decel); } /// /// Parameter Write RAM /// /// /// /// /// public bool WriteRamIntValue(byte lowByte,byte highByte,int intValue) { _sendTime = DateTime.Now; return LinMotDeviceConfigManager.Instance.SendWriteRamIntValue(Module.ToString(), _linmotId, lowByte, highByte, intValue); } /// /// 发送Command Table /// /// /// public bool SendCommandTableOperation(short entryId) { _motorTime = DateTime.Now; _sendTime = DateTime.Now; return LinMotDeviceConfigManager.Instance.SendCommandTable(Module.ToString(), _linmotId, entryId); } /// /// 发送GAI GoToPositionAfterActualCommand /// /// /// /// /// public bool SendGAIGoToPositionAfterActualCommand(int position, int speed, int accel, int decel) { _motorTime = DateTime.Now; _sendTime = DateTime.Now; return LinMotDeviceConfigManager.Instance.SendVAIGotoPositionAfterActualCommand(Module.ToString(), _linmotId,position, speed, accel, decel); } /// /// 停止 /// /// /// /// public bool StopOperation(string cmd, object[] args) { if (_currentOperation==LinMotOperation.StopMotor) { LOG.WriteLog(eEvent.WARN_LINMOT, Module, "current operation is stop motor,cannot repet stop motor"); return false; } if (_currentOperation==LinMotOperation.StartVAIGoToPosition) { _startVAIPositionRoutine.Abort(); } _status = RState.Running; _currentOperation = LinMotOperation.StopMotor; _stopMotorRoutine.Start(LastSwitchOff); _direction = ""; return true; } /// /// 定时器 /// /// public bool OnTimer() { if (_status == RState.Running) { IRoutine routine = GetCurrentRoutine(); if (routine != null) { RState rsState = routine.Monitor(); if (rsState == RState.Failed || rsState == RState.Timeout) { _status = RState.Failed; _currentOperation = LinMotOperation.None; } else if (rsState == RState.End) { _status = RState.End; _currentOperation = LinMotOperation.None; } } } SendReadStatusCommand(); if(DateTime.Now.Subtract(_statusWordDateTime).TotalSeconds>=2) { _statusWord = 0; UpdateStatusWord(_statusWord); } return true; } /// /// 当前Routine; /// /// private IRoutine GetCurrentRoutine() { switch(_currentOperation) { case LinMotOperation.StartVAIGoToPosition: return _startVAIPositionRoutine; case LinMotOperation.StartCurve: return _startContinueCurveRoutine; case LinMotOperation.Reset: return _resetRoutine; case LinMotOperation.StopMotor: return _stopMotorRoutine; default: return null; } } /// /// 监控 /// public void Monitor() { } /// /// 发送读取状态命令 /// private void SendReadStatusCommand() { if (LinMotDeviceConfigManager.Instance.GetDeviceConnect(Module.ToString())) { if (DateTime.Now.Subtract(_sendTime).TotalMilliseconds >= 300) { SendOperation(LinMotOperation.ReadStatus); } } else { _statusWord = 0; UpdateStatusWord(_statusWord); } } /// /// 发送消息 /// /// public bool SendOperation(LinMotOperation operation) { _sendTime = DateTime.Now; return LinMotDeviceConfigManager.Instance.SendOperation(Module.ToString(), _linmotId,operation); } /// /// 掉电 /// /// public bool SwitchOff() { return SendOperation(LinMotOperation.SwitchOff); } /// /// 中止 /// public void AbortCurrentRoutine() { StopOperation("", null); } public bool Initialize() { return true; } public void Terminate() { StopOperation("", null); } public void Reset() { } } }