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()
{
}
}
}