JetAxisBase.cs 57 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501
  1. using Aitex.Core.RT.DataCenter;
  2. using Aitex.Core.RT.Device;
  3. using Aitex.Core.RT.Event;
  4. using Aitex.Core.RT.Log;
  5. using Aitex.Core.RT.OperationCenter;
  6. using Aitex.Core.RT.Routine;
  7. using Aitex.Core.RT.SCCore;
  8. using Aitex.Core.Util;
  9. using MECF.Framework.Common.Beckhoff.AxisProvider;
  10. using MECF.Framework.Common.Beckhoff.IOAxis;
  11. using MECF.Framework.Common.Beckhoff.Station;
  12. using MECF.Framework.Common.CommonData.PUF;
  13. using MECF.Framework.Common.TwinCat;
  14. using MECF.Framework.Common.Utilities;
  15. using CyberX8_Core;
  16. using System;
  17. using System.Collections.Generic;
  18. using System.Linq;
  19. using System.Reflection;
  20. using System.Text;
  21. using System.Threading;
  22. using System.Threading.Tasks;
  23. using System.Timers;
  24. using CommunityToolkit.HighPerformance.Buffers;
  25. using System.Windows.Documents;
  26. using MECF.Framework.Common.Device.Galil;
  27. namespace CyberX8_RT.Devices.AXIS
  28. {
  29. public abstract class JetAxisBase : BaseDevice, IDevice
  30. {
  31. #region 常量
  32. private const string STATUS_WORD = "StatusWord";
  33. private const string DIGITAL_INPUTS="DigitalInputs";
  34. private const string CONTROL_WORD = "ControlWord";
  35. private const string MODE_OF_OPERATION = "ModeOfOperation";
  36. protected const string PROFILE_VELOCITY = "ProfileVelocity";
  37. protected const string PROFILE_ACCEL = "ProfileAccel";
  38. protected const string PROFILE_DECEL = "ProfileDecel";
  39. private const string TARGET_POSITION = "TargetPosition";
  40. private const string MOTOR_POSITION = "MotorPosition";
  41. private const string ACTUAL_VELOCITY = "ActualVelocity";
  42. private const string ACTUAL_TORQUE = "ActualTorque";
  43. private const string POSITION_ERROR = "PositionError";
  44. private const string MOTION_DATA = "MotionData";
  45. private const string CURRENT_STATION = "CurrentStation";
  46. protected const string CURRENT_STATION_LIST = "CurrentStationList";
  47. private const string IS_SWITCH_ON = "IsSwitchOn";
  48. private const string HOME_OFFSET = "HomeOffset";
  49. private const string HOMING_METHOD = "HomingMethod";
  50. protected const string HOMING_VELOCITY = "HomingVelocity";
  51. protected const string HOMING_VELOCITY_SLOW = "HomingVelocitySlow";
  52. protected const string HOMING_ACCEL = "HomingAccel";
  53. private const string NEGATIVE_TORQUE_LIMIT="NegativeTorqueLimit";
  54. private const string POSITIVE_TORQUE_LIMIT = "PositiveTorqueLimit";
  55. private const string MANUF_STATUS="ManufStatus";
  56. private const string SOFTWARE_LIMIT_MINUS= "SoftwareLimitMinus";
  57. private const string SOFTWARE_LIMIT_PLUS = "SoftwareLimitPlus";
  58. private const string STOP_CODE = "StopCode";
  59. #endregion
  60. #region 内部变量
  61. /// <summary>
  62. /// 比例因子
  63. /// </summary>
  64. private double _scaleFactor = 0;
  65. /// <summary>
  66. /// jog限制
  67. /// </summary>
  68. private double _jogLimit = 0;
  69. /// <summary>
  70. /// 当前位置数值(用于判定是否正在运动)
  71. /// </summary>
  72. private int _currentLocation = 0;
  73. /// <summary>
  74. /// 是否运动
  75. /// </summary>
  76. private bool _isRun = false;
  77. /// <summary>
  78. /// 当前位置
  79. /// </summary>
  80. private string _currentStation = "";
  81. /// <summary>
  82. /// 当前位置集合(多个位置共用一个数值)
  83. /// </summary>
  84. private List<string> _currentStationList = new List<string>();
  85. /// <summary>
  86. /// 当前位置锁
  87. /// </summary>
  88. private object _locationLocker = new object();
  89. /// <summary>
  90. /// 工位位置对象
  91. /// </summary>
  92. private BeckhoffStationAxis _stationAxis;
  93. /// <summary>
  94. /// 运动时间
  95. /// </summary>
  96. private DateTime _runTime = DateTime.Now;
  97. /// <summary>
  98. /// 尺寸
  99. /// </summary>
  100. private int _waferSize = 0;
  101. /// <summary>
  102. /// inter lock
  103. /// </summary>
  104. private IAxisInterLock _interLock;
  105. /// <summary>
  106. /// 是否存在Rev Sensor Limit
  107. /// </summary>
  108. private bool _isRevSensorLimit = false;
  109. /// <summary>
  110. /// 是否存在Forward Limit
  111. /// </summary>
  112. private bool _isForwardSensorLimit = false;
  113. /// <summary>
  114. /// Home Switched是否触发
  115. /// </summary>
  116. private bool _isHomeSwitchedTrigger = false;
  117. /// <summary>
  118. /// 变量是否初始化字典
  119. /// </summary>
  120. private Dictionary<string, bool> _variableInitializeDic = new Dictionary<string, bool>();
  121. #endregion
  122. #region protected 字段
  123. /// <summary>
  124. /// 轴参数对象
  125. /// </summary>
  126. protected GalilAxisConfig _galilAxisConfig = null;
  127. /// <summary>
  128. /// 状态
  129. /// </summary>
  130. protected RState _status;
  131. /// <summary>
  132. /// 当前操作
  133. /// </summary>
  134. protected MotionOperation _currentOperation = MotionOperation.None;
  135. /// <summary>
  136. /// 模式
  137. /// </summary>
  138. protected byte _modeOfOperation;
  139. /// <summary>
  140. /// 状态字
  141. /// </summary>
  142. protected ushort _statusWord;
  143. /// <summary>
  144. /// 控制字
  145. /// </summary>
  146. protected ushort _controlWord;
  147. /// <summary>
  148. /// 运动数据对象
  149. /// </summary>
  150. protected CommandMotionData _commandMotionData = new CommandMotionData();
  151. /// <summary>
  152. /// Home状态
  153. /// </summary>
  154. protected bool _isHomed;
  155. /// <summary>
  156. /// SwitchOn状态
  157. /// </summary>
  158. protected bool _isSwitchOn;
  159. /// <summary>
  160. /// 是否错误
  161. /// </summary>
  162. protected bool _isError;
  163. /// <summary>
  164. /// 是否到达目标位置
  165. /// </summary>
  166. protected bool _inTargetPosition = false;
  167. /// <summary>
  168. /// 初始化的速度
  169. /// </summary>
  170. protected int _initialVelocity = 0;
  171. /// <summary>
  172. /// 初始化的加速度
  173. /// </summary>
  174. protected int _initialAcceleration = 0;
  175. /// <summary>
  176. /// 初始化的减速度
  177. /// </summary>
  178. protected int _initialDeceleration = 0;
  179. /// <summary>
  180. /// 运动速度
  181. /// </summary>
  182. protected int _profileVelocity = 0;
  183. /// <summary>
  184. /// 运动加速度
  185. /// </summary>
  186. protected int _profileAcceleration = 0;
  187. /// <summary>
  188. /// 运动减速度
  189. /// </summary>
  190. protected int _profileDeceleration = 0;
  191. /// <summary>
  192. /// 负向Torque限制
  193. /// </summary>
  194. protected int _profileNegativeTorqueLimit = 0;
  195. /// <summary>
  196. /// 正向Torque限制
  197. /// </summary>
  198. protected int _profilePositiveTorqueLimit = 0;
  199. /// <summary>
  200. /// Homing 速度
  201. /// </summary>
  202. protected int _profileHomingVelocity = 0;
  203. /// <summary>
  204. /// Homing 速度Slow
  205. /// </summary>
  206. protected int _profileHomingVelocitySlow = 0;
  207. /// <summary>
  208. /// Homing加速度
  209. /// </summary>
  210. protected int _profileHomingAccel = 0;
  211. /// <summary>
  212. /// <summary>
  213. /// 目标位置
  214. /// </summary>
  215. protected double _targetPosition = 0.0;
  216. /// <summary>
  217. /// home超时时长
  218. /// </summary>
  219. protected int _homeTimeout = 5000;
  220. /// <summary>
  221. /// coe输出变量集合
  222. /// </summary>
  223. protected List<string> _coeOutputs = new List<string>();
  224. /// <summary>
  225. /// 速度比例
  226. /// </summary>
  227. protected int _speedRatio = 1;
  228. /// <summary>
  229. /// 加速度的比例
  230. /// </summary>
  231. protected int _accelerationRatio = 1;
  232. /// <summary>
  233. /// torque比例
  234. /// </summary>
  235. protected int _torqueRatio = 1000;
  236. /// <summary>
  237. /// Motion Position比例
  238. /// </summary>
  239. protected double _motionPositionRation = 1;
  240. #endregion
  241. #region 属性
  242. /// <summary>
  243. /// 状态
  244. /// </summary>
  245. public RState Status { get { return _status; } }
  246. /// <summary>
  247. /// Home状态
  248. /// </summary>
  249. public bool IsHomed { get { return _isHomed; } set { _isHomed = value; } }
  250. /// <summary>
  251. /// SwitchOn状态
  252. /// </summary>
  253. public bool IsSwitchOn { get { return _isSwitchOn; } }
  254. /// <summary>
  255. /// 模式
  256. /// </summary>
  257. public byte ModeOfOperation { get { return _modeOfOperation; } }
  258. /// <summary>
  259. /// 控制字
  260. /// </summary>
  261. public ushort ControlWord { get { return _controlWord; } }
  262. /// <summary>
  263. /// 是否运动中
  264. /// </summary>
  265. public bool IsRun { get { return _isRun; } }
  266. /// <summary>
  267. /// 当前位置
  268. /// </summary>
  269. public string CurrentStation { get { return _currentStation; } }
  270. /// <summary>
  271. /// 是否到达目标位置
  272. /// </summary>
  273. public bool InTargetPosition { get { return _inTargetPosition; } }
  274. /// <summary>
  275. /// 运动数据对象
  276. /// </summary>
  277. public CommandMotionData MotionData { get { return _commandMotionData; } }
  278. /// <summary>
  279. /// 负向Torque限制数值
  280. /// </summary>
  281. public int NegativeTorqueLimit { get { return _profileNegativeTorqueLimit; } }
  282. /// <summary>
  283. /// 正向Torque限制数值
  284. /// </summary>
  285. public int PositiveTorqueLimit { get { return _profilePositiveTorqueLimit; } }
  286. /// <summary>
  287. /// 是否错误
  288. /// </summary>
  289. public bool IsError { get { return _isError; } }
  290. /// <summary>
  291. /// 目标位置
  292. /// </summary>
  293. public double TargetPosition { get { return _targetPosition; } }
  294. /// <summary>
  295. /// 加速度
  296. /// </summary>
  297. public double ProfileAcceleration { get { return _profileAcceleration; } }
  298. /// <summary>
  299. /// 减速度
  300. /// </summary>
  301. public double ProfileDeceleration { get { return _profileDeceleration; } }
  302. /// <summary>
  303. /// inter lock接口对象
  304. /// </summary>
  305. public IAxisInterLock InterLock { set { _interLock= value; } }
  306. /// <summary>
  307. /// 是否存在Sensor Limit
  308. /// </summary>
  309. public bool IsRevSensorLimit { set { _isRevSensorLimit = value; } }
  310. /// <summary>
  311. /// 是否存在正向Sensor Limit
  312. /// </summary>
  313. public bool IsForwardSensorLimit { set { _isForwardSensorLimit = value; } }
  314. /// <summary>
  315. /// Home Switch是否触发
  316. /// </summary>
  317. public bool IsHomeSwitchedTriggered { get { return _isHomeSwitchedTrigger; } set { _isHomeSwitchedTrigger = value; } }
  318. /// <summary>
  319. /// 所有io变量是否初始化
  320. /// </summary>
  321. public bool IOInitialized { get { return AllIoVariableInitialized(); } }
  322. /// <summary>
  323. /// WaferSize
  324. /// </summary>
  325. public int WaferSize { get { return _waferSize; } set { _waferSize = value; } }
  326. #endregion
  327. /// <summary>
  328. /// 构造函数
  329. /// </summary>
  330. /// <param name="moduleName"></param>
  331. /// <param name="name"></param>
  332. public JetAxisBase(string moduleName,string name) : base(moduleName, name,name,name)
  333. {
  334. InitializeParameter();
  335. LoadStation();
  336. InitializeRoutine();
  337. SubscribeData();
  338. InitializeOperation();
  339. }
  340. #region private方法
  341. /// <summary>
  342. /// 加载Station位置
  343. /// </summary>
  344. private void LoadStation()
  345. {
  346. _waferSize = SC.GetValue<int>("System.WaferSize");
  347. _stationAxis = BeckhoffStationLocationManager.Instance.GetStationAxis(Module, Name, _waferSize);
  348. }
  349. /// <summary>
  350. /// 订阅数据
  351. /// </summary>
  352. private void SubscribeData()
  353. {
  354. BeckhoffProviderAxis beckhoffProviderAxis = BeckhoffAxisProviderManager.Instance.GetAxisProvider($"{Module}.{Name}");
  355. if (beckhoffProviderAxis != null)
  356. {
  357. _scaleFactor = beckhoffProviderAxis.ScaleFactor;
  358. _jogLimit = beckhoffProviderAxis.JogLimit;
  359. }
  360. _galilAxisConfig = GalilControllerCfgManager.Instance.GetGalilAxisConfig(Module.ToString(),Name);
  361. if (_galilAxisConfig != null)
  362. {
  363. _profileVelocity =CalculateMultiplySpeedRatio(_galilAxisConfig.Speed);
  364. _initialVelocity = _profileVelocity;
  365. _profileAcceleration = CalculateDivideAccelerationRatio(_galilAxisConfig.Acceleration);
  366. _initialAcceleration = _profileAcceleration;
  367. _profileDeceleration = CalculateDivideAccelerationRatio(_galilAxisConfig.Deceleration);
  368. _initialDeceleration = _profileDeceleration;
  369. _profileNegativeTorqueLimit = _galilAxisConfig.NegativeTorqueLimit;
  370. _profilePositiveTorqueLimit = _galilAxisConfig.PositiveTorqueLimit;
  371. _commandMotionData.FileAcceleration = CalculateValueAfterScale(_galilAxisConfig.Acceleration);
  372. _commandMotionData.FileDeceleration = CalculateValueAfterScale(_galilAxisConfig.Deceleration);
  373. _commandMotionData.HomeOffset = CalculateValueAfterScale(_galilAxisConfig.HomingOffset);
  374. _commandMotionData.FileHomingAccel = CalculateValueAfterScale(_galilAxisConfig.HomingAcceleration);
  375. _commandMotionData.FileHomingVelocitySlow = CalculateValueAfterScale(_galilAxisConfig.HomingSpeed);
  376. _commandMotionData.FileHomingVelocity = CalculateValueAfterScale(_galilAxisConfig.HomingSpeed);
  377. _commandMotionData.FileProfileVelocity = CalculateValueAfterScale(_galilAxisConfig.Speed);
  378. _commandMotionData.FwdSoftLimit = CalculateValueAfterScale(_galilAxisConfig.ForwardSoftwareLimit)-_commandMotionData.HomeOffset;
  379. _commandMotionData.RevSoftLimit = CalculateValueAfterScale(_galilAxisConfig.ReverseSoftwareLimit)-_commandMotionData.HomeOffset;
  380. _commandMotionData.NegativeTorqueLimit = _galilAxisConfig.NegativeTorqueLimit;
  381. _commandMotionData.PositiveTorqueLimit = _galilAxisConfig.PositiveTorqueLimit;
  382. if (_galilAxisConfig.NegativeTorqueLimit != 0 || _galilAxisConfig.PositiveTorqueLimit != 0)
  383. {
  384. _commandMotionData.TorqueLimit = $"-{_galilAxisConfig.NegativeTorqueLimit}/+{_galilAxisConfig.PositiveTorqueLimit}";
  385. }
  386. if(_galilAxisConfig.HomingTimeOut!=0)
  387. {
  388. _homeTimeout = _galilAxisConfig.HomingTimeOut;
  389. }
  390. }
  391. DATA.Subscribe($"{Module}.{Name}.{MOTION_DATA}", () => _commandMotionData,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  392. DATA.Subscribe($"{Module}.{Name}.{MOTOR_POSITION}", () => _commandMotionData.MotorPosition,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  393. DATA.Subscribe($"{Module}.{Name}.ProfileVelocity", () => _commandMotionData.ProfileVelocity, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  394. DATA.Subscribe($"{Module}.{Name}.Acceleration",()=>_commandMotionData.FileAcceleration,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  395. DATA.Subscribe($"{Module}.{Name}.Deceleration",()=>_commandMotionData.FileDeceleration,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  396. DATA.Subscribe($"{Module}.{Name}.ActualTorque",()=>_commandMotionData.ActualTorque,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  397. DATA.Subscribe($"{Module}.{Name}.{CURRENT_STATION}", () => _currentStation,SubscriptionAttribute.FLAG.IgnoreSaveDB);
  398. DATA.Subscribe($"{Module}.{Name}.{CURRENT_STATION_LIST}", () => _currentStationList, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  399. DATA.Subscribe($"{Module}.{Name}.{IS_SWITCH_ON}", () => IsSwitchOn, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  400. DATA.Subscribe($"{Module}.{Name}.IsHomed", () => IsHomed, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  401. DATA.Subscribe($"{Module}.{Name}.IsError",()=>IsError, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  402. DATA.Subscribe($"{Module}.{Name}.IsMoving", () => _isRun, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  403. DATA.Subscribe($"{Module}.{Name}.IsRun", () => _isRun, SubscriptionAttribute.FLAG.IgnoreSaveDB);
  404. }
  405. /// <summary>
  406. /// 初始化操作
  407. /// </summary>
  408. private void InitializeOperation()
  409. {
  410. OP.Subscribe($"{Module}.{Name}.{MotionOperation.SwitchOn}", (cmd, args) => { SwitchOn(); return true; });
  411. OP.Subscribe($"{Module}.{Name}.{MotionOperation.SwitchOff}", (cmd, args) => { SwitchOff(); return true; });
  412. OP.Subscribe($"{Module}.{Name}.{MotionOperation.Home}", (cmd, args) => { Home(); return true; });
  413. OP.Subscribe($"{Module}.{Name}.JogUp", JogUpPosition);
  414. OP.Subscribe($"{Module}.{Name}.JogDown", JogDownPosition);
  415. OP.Subscribe($"{Module}.{Name}.{MotionOperation.KeyDown}", KeyDownOperation);
  416. OP.Subscribe($"{Module}.{Name}.{MotionOperation.Stop}",(cmd,args)=> { return StopPositionOperation(); });
  417. OP.Subscribe($"{Module}.{Name}.{MotionOperation.Save}", SaveOperation);
  418. OP.Subscribe($"{Module}.{Name}.GotoSavedPosition", (cmd, args) => {
  419. return PositionStation(args[1].ToString()); });
  420. }
  421. /// <summary>
  422. /// 保存操作
  423. /// </summary>
  424. /// <param name="cmd"></param>
  425. /// <param name="args"></param>
  426. /// <returns></returns>
  427. public bool SaveOperation(string cmd, object[] args)
  428. {
  429. if (args.Length >= 2)
  430. {
  431. string key = args[0].ToString();
  432. double paramValue = double.Parse(args[1].ToString());
  433. BeckhoffStationLocationManager.Instance.SaveMotionPosition(key, paramValue);
  434. lock (_locationLocker)
  435. {
  436. List<string> lst = new List<string>();
  437. foreach (Station item in _stationAxis.Stations)
  438. {
  439. if (item.Name == key)
  440. {
  441. item.Position = paramValue.ToString();
  442. _currentStation = item.Name;
  443. if (!lst.Contains(item.Name))
  444. {
  445. lst.Add(item.Name);
  446. }
  447. }
  448. }
  449. _currentStationList.Clear();
  450. if (lst.Count != 0)
  451. {
  452. _currentStationList.AddRange(lst);
  453. }
  454. }
  455. LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", "save success");
  456. }
  457. return true;
  458. }
  459. /// <summary>
  460. /// 计算所处当前工位
  461. /// </summary>
  462. private void CalculateCurrentStation(double motor)
  463. {
  464. lock (_locationLocker)
  465. {
  466. List<string> tmp = _currentStationList.ToList();
  467. List<string> lst = new List<string>();
  468. foreach (Station station in _stationAxis.Stations)
  469. {
  470. if (double.TryParse(station.Position, out double value))
  471. {
  472. if (Math.Round(Math.Abs(motor - value),2) <= _stationAxis.ToleranceDefault)
  473. {
  474. _currentStation = station.Name;
  475. if (!lst.Contains(station.Name))
  476. {
  477. lst.Add(station.Name);
  478. }
  479. }
  480. }
  481. }
  482. _currentStationList.Clear();
  483. if (lst.Count != 0)
  484. {
  485. string str = string.Join(",", tmp);
  486. _currentStationList.AddRange(lst);
  487. string strLst = string.Join(",", _currentStationList);
  488. if (str != strLst)
  489. {
  490. LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", $"position {motor} current {strLst}");
  491. }
  492. }
  493. else
  494. {
  495. _currentStation = "";
  496. }
  497. if (tmp.Count != 0&&_currentStationList.Count==0)
  498. {
  499. LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", $"position {motor} current is empty");
  500. }
  501. }
  502. }
  503. /// <summary>
  504. /// 文本框回车操作
  505. /// </summary>
  506. /// <param name="cmd"></param>
  507. /// <param name="args"></param>
  508. /// <returns></returns>
  509. private bool KeyDownOperation(string cmd, object[] args)
  510. {
  511. if (args.Length < 2)
  512. {
  513. return false;
  514. }
  515. if (double.TryParse(args[1].ToString(), out double value))
  516. {
  517. AxisKeyDown(args[0].ToString(), value);
  518. }
  519. else
  520. {
  521. EV.PostWarningLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{args[0]} value {args[1]}is not int value");
  522. }
  523. return true;
  524. }
  525. /// <summary>
  526. /// 是否所有IO变量初始化完成
  527. /// </summary>
  528. /// <returns></returns>
  529. private bool AllIoVariableInitialized()
  530. {
  531. foreach (string item in _variableInitializeDic.Keys)
  532. {
  533. if (!_variableInitializeDic[item])
  534. {
  535. LOG.WriteLog(eEvent.ERR_DRYER, Module, $"{item} is not initialized");
  536. return false;
  537. }
  538. }
  539. return true;
  540. }
  541. /// <summary>
  542. /// 更新运动数据
  543. /// </summary>
  544. /// <param name="variable"></param>
  545. /// <param name="value"></param>
  546. private void UpdateMotionData(string variable, object value)
  547. {
  548. if(!MotionData.IsDataInitialized)
  549. {
  550. MotionData.IsDataInitialized = true;
  551. }
  552. PropertyInfo property = MotionData.GetType().GetProperty(variable);
  553. if (property != null)
  554. {
  555. if (JudgeIsScale(variable))
  556. {
  557. if (int.TryParse(value.ToString(), out int intValue))
  558. {
  559. if (JudgeSpeedRatio(variable))
  560. {
  561. property.SetValue(MotionData, CalculateValueAfterScale(CalculateDivideSpeedRatio(intValue)));
  562. }
  563. else if(JudgeAccelerationRation(variable))
  564. {
  565. property.SetValue(MotionData, CalculateValueAfterScale(CalculateMultiplyAccelerationRatio(intValue)));
  566. }
  567. else
  568. {
  569. if (variable == MOTOR_POSITION)
  570. {
  571. intValue = (int)Math.Round(intValue * _motionPositionRation,0);
  572. }
  573. property.SetValue(MotionData, CalculateValueAfterScale(intValue));
  574. }
  575. }
  576. else
  577. {
  578. property.SetValue(MotionData, value);
  579. }
  580. }
  581. else if(variable==ACTUAL_TORQUE)
  582. {
  583. if (short.TryParse(value.ToString(), out short shortValue))
  584. {
  585. property.SetValue(MotionData, CalculateDivideTorqueRatio(shortValue));
  586. }
  587. else
  588. {
  589. property.SetValue(MotionData, value);
  590. }
  591. }
  592. else
  593. {
  594. property.SetValue(MotionData, value);
  595. }
  596. }
  597. if (variable == IS_SWITCH_ON)
  598. {
  599. _isSwitchOn = MotionData.IsSwitchOn;
  600. }
  601. }
  602. /// <summary>
  603. /// 判定是否需要比例计算
  604. /// </summary>
  605. /// <param name="variable"></param>
  606. /// <returns></returns>
  607. private bool JudgeIsScale(string variable)
  608. {
  609. switch (variable)
  610. {
  611. case PROFILE_VELOCITY:
  612. case PROFILE_ACCEL:
  613. case PROFILE_DECEL:
  614. case MOTOR_POSITION:
  615. case POSITION_ERROR:
  616. case ACTUAL_VELOCITY:
  617. case TARGET_POSITION:
  618. case HOMING_ACCEL:
  619. case HOMING_VELOCITY:
  620. case HOMING_VELOCITY_SLOW:
  621. case HOME_OFFSET:
  622. return true;
  623. default:
  624. return false;
  625. }
  626. }
  627. /// <summary>
  628. /// 判定是否需要速度调整比例
  629. /// </summary>
  630. /// <param name="variable"></param>
  631. /// <returns></returns>
  632. private bool JudgeSpeedRatio(string variable)
  633. {
  634. switch (variable)
  635. {
  636. case PROFILE_VELOCITY:
  637. case ACTUAL_VELOCITY:
  638. case HOMING_VELOCITY:
  639. case HOMING_VELOCITY_SLOW:
  640. case HOME_OFFSET:
  641. return true;
  642. default:
  643. return false;
  644. }
  645. }
  646. /// <summary>
  647. /// 是否是加速度调整比例
  648. /// </summary>
  649. /// <param name="variable"></param>
  650. /// <returns></returns>
  651. protected bool JudgeAccelerationRation(string variable)
  652. {
  653. switch(variable)
  654. {
  655. case PROFILE_ACCEL:
  656. case PROFILE_DECEL:
  657. case HOMING_ACCEL:
  658. return true;
  659. default:
  660. return false;
  661. }
  662. }
  663. /// <summary>
  664. /// motor position发生变化
  665. /// </summary>
  666. /// <param name="location"></param>
  667. private void MotionPositionChanged(int location)
  668. {
  669. if(Math.Round(Math.Abs(location - _currentLocation)/_scaleFactor, 2)>0)
  670. //if (Math.Abs(location - _currentLocation)/_scaleFactor >= _stationAxis.ToleranceDefault)
  671. {
  672. _currentLocation = location;
  673. _runTime = DateTime.Now;
  674. _isRun = true;
  675. }
  676. }
  677. /// <summary>
  678. /// 更新Digital Inputs
  679. /// </summary>
  680. /// <param name="digitalInputs"></param>
  681. private void UpdateDigitalInputs(uint digitalInputs)
  682. {
  683. if (_isRevSensorLimit)
  684. {
  685. MotionData.RevLimited = !(((digitalInputs>>18) & 0x01) == 0x01);
  686. }
  687. if(_isForwardSensorLimit)
  688. {
  689. MotionData.FwdLimited = !(((digitalInputs >> 17) & 0x01) == 0x01);
  690. }
  691. MotionData.HomedSwitched =((digitalInputs>>22)&0x01) == 0x01;
  692. if(!_isHomeSwitchedTrigger&&MotionData.HomedSwitched)
  693. {
  694. _isHomeSwitchedTrigger = true;
  695. }
  696. }
  697. /// <summary>
  698. /// 更新ManufactureStatus
  699. /// </summary>
  700. /// <param name="manufactureStatus"></param>
  701. private void UpdateManufactureStatus(uint manufactureStatus)
  702. {
  703. MotionData.HomedSwitched = ((manufactureStatus >> 26)&0x01) == 0x01;
  704. if (!_isHomeSwitchedTrigger && MotionData.HomedSwitched)
  705. {
  706. _isHomeSwitchedTrigger = true;
  707. }
  708. }
  709. /// <summary>
  710. /// 更新Torque Limited状态
  711. /// </summary>
  712. private void UpdateTorqueLimited()
  713. {
  714. if(MotionData.NegativeTorqueLimit!=0||MotionData.PositiveTorqueLimit!=0)
  715. {
  716. MotionData.TorqueLimited = (MotionData.ActualTorque >= -MotionData.NegativeTorqueLimit) && (MotionData.ActualTorque <= MotionData.PositiveTorqueLimit) ;
  717. }
  718. else
  719. {
  720. MotionData.TorqueLimited = true;
  721. }
  722. }
  723. /// <summary>
  724. /// 根据位置获取相应的Position数值
  725. /// </summary>
  726. /// <param name="station"></param>
  727. /// <returns></returns>
  728. public (bool success,double position) GetPositionByStation(string station)
  729. {
  730. foreach(Station item in _stationAxis.Stations)
  731. {
  732. if(item.Name.EndsWith(station))
  733. {
  734. if(double.TryParse(item.Position,out double position))
  735. {
  736. return (true, position);
  737. }
  738. else
  739. {
  740. return (false, 0);
  741. }
  742. }
  743. }
  744. return (false, 0);
  745. }
  746. #endregion
  747. #region protected 子类使用共用类,子类不再扩展
  748. /// <summary>
  749. /// 计算比例后的数值
  750. /// </summary>
  751. /// <param name="value"></param>
  752. /// <returns></returns>
  753. protected double CalculateValueAfterScale(int value)
  754. {
  755. if (_scaleFactor != 0)
  756. {
  757. return Math.Round((double)value / _scaleFactor, 2);
  758. }
  759. else
  760. {
  761. return (double)value;
  762. }
  763. }
  764. /// <summary>
  765. /// 确认操作状态
  766. /// </summary>
  767. /// <param name="operation"></param>
  768. protected void ConfirmOperationState(MotionOperation operation)
  769. {
  770. if (_currentOperation == operation)
  771. {
  772. _status = RState.End;
  773. EndOperation();
  774. }
  775. }
  776. /// <summary>
  777. /// 结束操作
  778. /// </summary>
  779. protected void EndOperation()
  780. {
  781. LOG.WriteLog(eEvent.INFO_AXIS,$"{Module}.{Name}", $"{Module}.{Name} execute {_currentOperation} complete");
  782. _currentOperation = MotionOperation.None;
  783. _targetPosition = 0;
  784. }
  785. /// <summary>
  786. /// 订阅变量数值发生变化
  787. /// </summary>
  788. protected void SubscribeValueAction()
  789. {
  790. GalilAxisSubscribeUpdateVariable(IS_SWITCH_ON);
  791. GalilAxisSubscribeUpdateVariable(STOP_CODE);
  792. GalilAxisSubscribeUpdateVariable( MOTOR_POSITION);
  793. GalilAxisSubscribeUpdateVariable( POSITION_ERROR);
  794. GalilAxisSubscribeUpdateVariable( ACTUAL_TORQUE);
  795. GalilAxisSubscribeUpdateVariable( ACTUAL_VELOCITY);
  796. }
  797. /// <summary>
  798. /// 订阅IO变量
  799. /// </summary>
  800. /// <param name="variable"></param>
  801. private void GalilAxisSubscribeUpdateVariable(string variable)
  802. {
  803. _variableInitializeDic[variable] = false;
  804. GalilAxisManager.Instance.SubscribeModuleVariable($"{Module}.{Name}", variable, UpdateVariableValue);
  805. }
  806. /// <summary>
  807. /// 更新变量数值
  808. /// </summary>
  809. /// <param name="variable"></param>
  810. /// <param name="value"></param>
  811. protected void UpdateVariableValue(string variable, object value)
  812. {
  813. if (value == null)
  814. {
  815. return;
  816. }
  817. if (_variableInitializeDic.ContainsKey(variable) && !_variableInitializeDic[variable])
  818. {
  819. _variableInitializeDic[variable] = true;
  820. }
  821. if (variable == MOTOR_POSITION)
  822. {
  823. if (int.TryParse(value.ToString(), out int location))
  824. {
  825. MotionPositionChanged(location);
  826. if (_galilAxisConfig.ForwardSoftwareLimit != 0)
  827. {
  828. MotionData.ForwardSoftwareLimited = location > _galilAxisConfig.ForwardSoftwareLimit-_galilAxisConfig.HomingOffset;
  829. if (!_isForwardSensorLimit && !_isRevSensorLimit)
  830. {
  831. MotionData.FwdLimited = MotionData.ForwardSoftwareLimited;
  832. }
  833. }
  834. if (_galilAxisConfig.ReverseSoftwareLimit != 0)
  835. {
  836. MotionData.ReverseSoftwareLimited = location < _galilAxisConfig.ReverseSoftwareLimit-_galilAxisConfig.HomingOffset;
  837. if (!_isRevSensorLimit && !_isForwardSensorLimit)
  838. {
  839. MotionData.RevLimited = MotionData.ReverseSoftwareLimited;
  840. }
  841. }
  842. }
  843. }
  844. UpdateMotionData(variable, value);
  845. if (variable == MOTOR_POSITION)
  846. {
  847. CalculateCurrentStation(MotionData.MotorPosition);
  848. }
  849. if (variable == ACTUAL_TORQUE)
  850. {
  851. UpdateTorqueLimited();
  852. }
  853. else if (variable == POSITIVE_TORQUE_LIMIT || variable == NEGATIVE_TORQUE_LIMIT)
  854. {
  855. MotionData.TorqueLimit = $"-{MotionData.NegativeTorqueLimit}/+{MotionData.PositiveTorqueLimit}";
  856. }
  857. }
  858. /// <summary>
  859. /// 监控(用于判定是否停止运动)
  860. /// </summary>
  861. protected void JudgeRunMonitor()
  862. {
  863. if (_isRun && DateTime.Now.Subtract(_runTime).TotalMilliseconds >= 500)
  864. {
  865. _isRun = false;
  866. }
  867. }
  868. #endregion
  869. #region public 公开方法
  870. /// <summary>
  871. /// 初始化
  872. /// </summary>
  873. /// <returns></returns>
  874. public bool Initialize()
  875. {
  876. SubscribeValueAction();
  877. return true;
  878. }
  879. /// <summary>
  880. /// 当前位置是否离目标位置不远
  881. /// </summary>
  882. /// <param name="targetPosition"></param>
  883. /// <returns></returns>
  884. public bool JudgeCurrentPositionIsInTargetPosition(int targetPosition)
  885. {
  886. double scaledTargetPosition = targetPosition /_scaleFactor;
  887. double currentMotionPosition = MotionData.MotorPosition;
  888. double delta = Math.Round(Math.Abs(currentMotionPosition - scaledTargetPosition), 2);
  889. bool result = delta <= _stationAxis.ToleranceDefault;
  890. return result;
  891. }
  892. /// <summary>
  893. /// 计算乘以比例后的数值
  894. /// </summary>
  895. /// <param name="value"></param>
  896. /// <returns></returns>
  897. public int CalculateValueMultiplyScale(double value)
  898. {
  899. if (_scaleFactor != 0)
  900. {
  901. return (int)Math.Round(value * _scaleFactor, 0);
  902. }
  903. else
  904. {
  905. return (int)Math.Round(value, 0);
  906. }
  907. }
  908. /// <summary>
  909. /// 计算乘上速度比例后的速度
  910. /// </summary>
  911. /// <param name="speed"></param>
  912. /// <returns></returns>
  913. public int CalculateMultiplySpeedRatio(int speed)
  914. {
  915. return speed * _speedRatio;
  916. }
  917. /// <summary>
  918. /// 计算乘上加速度比例后的加速度
  919. /// </summary>
  920. /// <param name="acceleration"></param>
  921. /// <returns></returns>
  922. public int CalculateMultiplyAccelerationRatio(int acceleration)
  923. {
  924. return acceleration * _accelerationRatio;
  925. }
  926. /// <summary>
  927. /// 计算除以速度比例后的速度
  928. /// </summary>
  929. /// <param name="speed"></param>
  930. /// <returns></returns>
  931. protected int CalculateDivideSpeedRatio(int speed)
  932. {
  933. return speed / _speedRatio;
  934. }
  935. /// <summary>
  936. /// 计算除以加速度比例后的加速度
  937. /// </summary>
  938. /// <param name="acceleration"></param>
  939. /// <returns></returns>
  940. protected int CalculateDivideAccelerationRatio(int acceleration)
  941. {
  942. return acceleration / _accelerationRatio;
  943. }
  944. /// <summary>
  945. /// 计算除以Torque比例后的Torque
  946. /// </summary>
  947. /// <param name="speed"></param>
  948. /// <returns></returns>
  949. protected double CalculateDivideTorqueRatio(int torque)
  950. {
  951. return Math.Round((double)torque / _torqueRatio, 2);
  952. }
  953. /// <summary>
  954. /// Jog Up
  955. /// </summary>
  956. /// <param name="cmd"></param>
  957. /// <param name="args"></param>
  958. /// <returns></returns>
  959. public bool JogUpPosition(string cmd, object[] args)
  960. {
  961. double jog=(double)args[1];
  962. double currentPosition=MotionData.MotorPosition;
  963. if(_jogLimit!=0)
  964. {
  965. if(jog>_jogLimit)
  966. {
  967. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"Jog Value {jog} is over {_jogLimit}");
  968. return false;
  969. }
  970. }
  971. if (!_isSwitchOn)
  972. {
  973. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"Axis is switch off,cannot jog");
  974. return false;
  975. }
  976. return ProfilePositionOperation(Math.Round(currentPosition + jog,2));
  977. }
  978. /// <summary>
  979. /// Jog Down
  980. /// </summary>
  981. /// <param name="cmd"></param>
  982. /// <param name="args"></param>
  983. /// <returns></returns>
  984. public bool JogDownPosition(string cmd, object[] args)
  985. {
  986. double jog = (double)args[1];
  987. double currentPosition = MotionData.MotorPosition;
  988. if (_jogLimit != 0)
  989. {
  990. if (jog > _jogLimit)
  991. {
  992. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"Jog Value {jog} is over {_jogLimit}");
  993. return false;
  994. }
  995. }
  996. if(!_isSwitchOn)
  997. {
  998. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"Axis is switch off,cannot jog");
  999. return false;
  1000. }
  1001. return ProfilePositionOperation(Math.Round(currentPosition - jog,2));
  1002. }
  1003. /// <summary>
  1004. /// Profile position操作
  1005. /// </summary>
  1006. /// <param name="cmd"></param>
  1007. /// <param name="args"></param>
  1008. /// <returns></returns>
  1009. public bool ProfilePositionOperation(double position)
  1010. {
  1011. _targetPosition = position;
  1012. int targetPosition = CalculateValueMultiplyScale(_targetPosition);
  1013. if (_galilAxisConfig.ForwardSoftwareLimit != 0 && targetPosition > _galilAxisConfig.ForwardSoftwareLimit - _galilAxisConfig.HomingOffset)
  1014. {
  1015. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"target position {_targetPosition} is over forward limit");
  1016. return false;
  1017. }
  1018. if (_galilAxisConfig.ReverseSoftwareLimit != 0 && targetPosition < _galilAxisConfig.ReverseSoftwareLimit - _galilAxisConfig.HomingOffset)
  1019. {
  1020. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"target position {_targetPosition} is less reverse limit");
  1021. return false;
  1022. }
  1023. bool result= ProfilePosition(targetPosition, _profileVelocity, _profileAcceleration, _profileDeceleration);
  1024. if (result)
  1025. {
  1026. MotionData.TargetPosition = _targetPosition;
  1027. }
  1028. return result;
  1029. }
  1030. /// <summary>
  1031. /// 移动至指定位置
  1032. /// </summary>
  1033. /// <param name="targetStation"></param>
  1034. /// <returns></returns>
  1035. public bool PositionStation(string targetStation,bool isHome=false,int velocity=0,int acceleration=0,int deceleration=0, bool judgeTorqueLimit = true)
  1036. {
  1037. if (!CheckPositionIsInStation(MotionData.MotorPosition,targetStation))
  1038. {
  1039. if(_interLock!=null&&!isHome&&!_interLock.CheckGotoPosition(targetStation))
  1040. {
  1041. return false;
  1042. }
  1043. var result=GetPositionByStation(targetStation);
  1044. if(result.success)
  1045. {
  1046. _targetPosition = result.position;
  1047. int targetPosition = (int)Math.Round(result.position*_scaleFactor, 0);
  1048. bool positionResult = false;
  1049. if (velocity != 0)
  1050. {
  1051. positionResult= ProfilePosition(targetPosition, velocity, acceleration, deceleration, judgeTorqueLimit);
  1052. }
  1053. else
  1054. {
  1055. positionResult= ProfilePosition(targetPosition, _profileVelocity, _profileAcceleration, _profileDeceleration, judgeTorqueLimit);
  1056. }
  1057. if (positionResult)
  1058. {
  1059. MotionData.TargetPosition = _targetPosition;
  1060. }
  1061. return positionResult;
  1062. }
  1063. LOG.WriteLog(eEvent.ERR_AXIS, Module, $"{targetStation} not in list,cannot goto fixed position");
  1064. return false;
  1065. }
  1066. else
  1067. {
  1068. _status = RState.End;
  1069. }
  1070. return true;
  1071. }
  1072. /// <summary>
  1073. /// 上电
  1074. /// </summary>
  1075. /// <returns></returns>
  1076. public bool WriteSwitchOn()
  1077. {
  1078. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SH", null);
  1079. }
  1080. /// <summary>
  1081. /// 下电
  1082. /// </summary>
  1083. /// <returns></returns>
  1084. public bool WriteSwitchOff()
  1085. {
  1086. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "MO", null);
  1087. }
  1088. /// <summary>
  1089. /// 写入相对位置
  1090. /// </summary>
  1091. /// <param name="referencePosition"></param>
  1092. /// <returns></returns>
  1093. public bool WriteReferencePosition(int referencePosition)
  1094. {
  1095. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "PR", referencePosition);
  1096. }
  1097. /// <summary>
  1098. /// 写入绝对位置
  1099. /// </summary>
  1100. /// <param name="referencePosition"></param>
  1101. /// <returns></returns>
  1102. public bool WriteAbsolutePosition(int absolutePosition)
  1103. {
  1104. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "PA", absolutePosition);
  1105. }
  1106. /// <summary>
  1107. /// 写入速度
  1108. /// </summary>
  1109. /// <param name="referencePosition"></param>
  1110. /// <returns></returns>
  1111. public bool WriteSpeed(int speed)
  1112. {
  1113. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "SP", speed);
  1114. }
  1115. /// <summary>
  1116. /// 写入加速度
  1117. /// </summary>
  1118. /// <param name="referencePosition"></param>
  1119. /// <returns></returns>
  1120. public bool WriteAcceleration(int acceleration)
  1121. {
  1122. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "AC", acceleration);
  1123. }
  1124. /// <summary>
  1125. /// 写入减速度
  1126. /// </summary>
  1127. /// <param name="referencePosition"></param>
  1128. /// <returns></returns>
  1129. public bool WriteDeceleration(int deceleration)
  1130. {
  1131. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "DC", deceleration);
  1132. }
  1133. /// <summary>
  1134. /// 停止
  1135. /// </summary>
  1136. /// <returns></returns>
  1137. public bool WriteStop()
  1138. {
  1139. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "ST", null);
  1140. }
  1141. /// <summary>
  1142. /// 开始运动
  1143. /// </summary>
  1144. /// <returns></returns>
  1145. public bool WriteStartMotion()
  1146. {
  1147. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "BG", null);
  1148. }
  1149. /// <summary>
  1150. /// Home 电机
  1151. /// </summary>
  1152. /// <returns></returns>
  1153. public bool WriteHomeAxisCommand()
  1154. {
  1155. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "HM", null);
  1156. }
  1157. /// <summary>
  1158. /// 手动置零
  1159. /// </summary>
  1160. /// <returns></returns>
  1161. public bool WriteDPZero()
  1162. {
  1163. bool result= GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "DP", 0);
  1164. if (result)
  1165. {
  1166. return GalilControllerCfgManager.Instance.SetAxisCommand(Module, Name, "DE", 0);
  1167. }
  1168. return false;
  1169. }
  1170. /// <summary>
  1171. /// 写入控制字
  1172. /// </summary>
  1173. /// <param name="controlWord"></param>
  1174. public bool WriteControlWord(ushort controlWord)
  1175. {
  1176. return BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{CONTROL_WORD}", controlWord);
  1177. }
  1178. /// <summary>
  1179. /// 写入ModeOfOperation
  1180. /// </summary>
  1181. /// <param name="modeOfOperation"></param>
  1182. /// <returns></returns>
  1183. public bool WriteModeOfMode(AxisModeOfOperation modeOfOperation)
  1184. {
  1185. return BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{MODE_OF_OPERATION}", (byte)modeOfOperation);
  1186. }
  1187. /// <summary>
  1188. /// 写变量数值
  1189. /// </summary>
  1190. /// <param name="variableName"></param>
  1191. /// <param name="value"></param>
  1192. /// <returns></returns>
  1193. public bool WriteVariable(string variableName,object value)
  1194. {
  1195. return BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{variableName}", value);
  1196. }
  1197. /// <summary>
  1198. /// 检验GotoPosition条件
  1199. /// </summary>
  1200. /// <param name="station"></param>
  1201. public bool CheckGotoPosition(string station)
  1202. {
  1203. return _interLock.CheckGotoPosition(station);
  1204. }
  1205. /// <summary>
  1206. /// 检验当位Position是否在位置上
  1207. /// </summary>
  1208. /// <param name="position"></param>
  1209. /// <param name="stationName"></param>
  1210. /// <returns></returns>
  1211. public bool CheckPositionIsInStation(double position,string stationName)
  1212. {
  1213. foreach (Station station in _stationAxis.Stations)
  1214. {
  1215. if (station.Name.ToLower().EndsWith(stationName.ToLower()))
  1216. {
  1217. if (double.TryParse(station.Position, out double value))
  1218. {
  1219. if (Math.Round(Math.Abs(position - value), 2) <= _stationAxis.ToleranceDefault)
  1220. {
  1221. return true;
  1222. }
  1223. }
  1224. }
  1225. }
  1226. return false;
  1227. }
  1228. /// <summary>
  1229. /// 检验位置是否为空
  1230. /// </summary>
  1231. /// <param name="position"></param>
  1232. /// <returns></returns>
  1233. public bool CheckPositionIsEmpty(double position)
  1234. {
  1235. foreach (Station station in _stationAxis.Stations)
  1236. {
  1237. if (double.TryParse(station.Position, out double value))
  1238. {
  1239. if (Math.Round(Math.Abs(position - value), 2) <= _stationAxis.ToleranceDefault)
  1240. {
  1241. return false;
  1242. }
  1243. }
  1244. }
  1245. return true;
  1246. }
  1247. /// <summary>
  1248. /// 判定与目标位置相对关系
  1249. /// </summary>
  1250. /// <param name="station"></param>
  1251. /// <param name="compareType">Left--位于目标位置左侧,Right--位于目标位置右侧</param>
  1252. /// <returns></returns>
  1253. public bool JudgeCompareTargetStation(string station,string compareType)
  1254. {
  1255. double position = 0;
  1256. var result = GetPositionByStation(station);
  1257. if(result.success)
  1258. {
  1259. position = result.position;
  1260. }
  1261. else
  1262. {
  1263. return false;
  1264. }
  1265. var buffer16Position=GetPositionByStation(station);
  1266. switch(compareType)
  1267. {
  1268. case "Left":
  1269. return IsInStationLeftDirection(position);
  1270. case "Right":
  1271. if (buffer16Position.success)
  1272. {
  1273. position=buffer16Position.position;
  1274. }
  1275. return IsInStationRightPosition(position);
  1276. default:
  1277. return false;
  1278. }
  1279. }
  1280. /// <summary>
  1281. /// 是否位于位置左侧
  1282. /// </summary>
  1283. /// <param name="stationPosition"></param>
  1284. /// <returns></returns>
  1285. private bool IsInStationLeftDirection(double stationPosition)
  1286. {
  1287. double currentPosition = MotionData.MotorPosition;
  1288. double targetPosition = _targetPosition;
  1289. if(_currentOperation==MotionOperation.Position)
  1290. {
  1291. return currentPosition <= stationPosition && targetPosition <= stationPosition;
  1292. }
  1293. else
  1294. {
  1295. return currentPosition <= stationPosition;
  1296. }
  1297. }
  1298. /// <summary>
  1299. /// 是否位于位置右侧
  1300. /// </summary>
  1301. /// <param name="stationPosition"></param>
  1302. /// <returns></returns>
  1303. private bool IsInStationRightPosition(double stationPosition)
  1304. {
  1305. double currentPosition = MotionData.MotorPosition;
  1306. double targetPosition = _targetPosition;
  1307. if (_currentOperation == MotionOperation.Position)
  1308. {
  1309. return currentPosition > stationPosition && targetPosition > stationPosition;
  1310. }
  1311. else
  1312. {
  1313. return currentPosition > stationPosition ;
  1314. }
  1315. }
  1316. #endregion
  1317. #region virtual共用方法,子类可实现更多方法
  1318. /// <summary>
  1319. /// Home共用方法
  1320. /// </summary>
  1321. /// <returns></returns>
  1322. public virtual bool Home()
  1323. {
  1324. if (!_isSwitchOn)
  1325. {
  1326. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_PUF, $"{Module}.{Name} switch off,cannot home");
  1327. return false;
  1328. }
  1329. if (_status == RState.Running)
  1330. {
  1331. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_PUF, $"{Module}.{Name} current execute {_currentOperation},cannot home");
  1332. return false;
  1333. }
  1334. _currentOperation = MotionOperation.Home;
  1335. _status = RState.Running;
  1336. return true;
  1337. }
  1338. /// <summary>
  1339. /// Home(isLogError)共用方法
  1340. /// </summary>
  1341. /// <returns></returns>
  1342. public virtual bool Home(bool isLogError)
  1343. {
  1344. if (!_isSwitchOn)
  1345. {
  1346. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_PUF, $"{Module}.{Name} switch off,cannot home");
  1347. return false;
  1348. }
  1349. if (_status == RState.Running)
  1350. {
  1351. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_PUF, $"{Module}.{Name} current execute {_currentOperation},cannot home");
  1352. return false;
  1353. }
  1354. _currentOperation = MotionOperation.Home;
  1355. _status = RState.Running;
  1356. return true;
  1357. }
  1358. /// <summary>
  1359. /// 更改速度百分比
  1360. /// </summary>
  1361. /// <param name="percent"></param>
  1362. /// <returns></returns>
  1363. public bool ChangePercentSpeedAceleration(int percent)
  1364. {
  1365. double percentSpeed = _initialVelocity * ((double)percent / 100);
  1366. double percentAceleration = _initialAcceleration * ((double)percent / 100);
  1367. double percentDeceleration = _initialDeceleration * ((double)percent / 100);
  1368. int changedSpeed = (int)(Math.Round(percentSpeed, 0));
  1369. int changedAcceleration = (int)(Math.Round(percentAceleration, 0));
  1370. int changedDeceleration = (int)(Math.Round(percentDeceleration, 0));
  1371. bool result= ChangeSpeedAcceleration(changedSpeed,changedAcceleration,changedDeceleration);
  1372. if(result)
  1373. {
  1374. _profileVelocity = changedSpeed;
  1375. _profileAcceleration = changedAcceleration;
  1376. _profileDeceleration = changedDeceleration;
  1377. }
  1378. return result;
  1379. }
  1380. #endregion
  1381. #region public abstract 子类实现方法
  1382. /// <summary>
  1383. /// 停止操作
  1384. /// </summary>
  1385. /// <param name="cmd"></param>
  1386. /// <param name="args"></param>
  1387. /// <returns></returns>
  1388. public abstract bool StopPositionOperation();
  1389. /// <summary>
  1390. /// change speed
  1391. /// </summary>
  1392. /// <param name="speed"></param>
  1393. /// <returns></returns>
  1394. public abstract bool ChangeSpeed(int speed);
  1395. /// <summary>
  1396. /// 改变速度加速度
  1397. /// </summary>
  1398. /// <param name="speed"></param>
  1399. /// <returns></returns>
  1400. public abstract bool ChangeSpeedAcceleration(int speed, int acceleration, int deceleration);
  1401. /// <summary>
  1402. /// Switch On
  1403. /// </summary>
  1404. public abstract bool SwitchOn();
  1405. /// <summary>
  1406. /// Switch Off
  1407. /// </summary>
  1408. public abstract bool SwitchOff();
  1409. /// <summary>
  1410. /// 停止
  1411. /// </summary>
  1412. public abstract void Stop();
  1413. /// <summary>
  1414. /// Enable Operation
  1415. /// </summary>
  1416. /// <returns></returns>
  1417. public abstract bool EnableOperation();
  1418. /// <summary>
  1419. /// 定时器执行
  1420. /// </summary>
  1421. /// <returns></returns>
  1422. public abstract bool OnTimer();
  1423. /// <summary>
  1424. /// Profile operation
  1425. /// </summary>
  1426. /// <param name="targetPoint"></param>
  1427. /// <param name="profileVelocity"></param>
  1428. /// <param name="profileAcceleration"></param>
  1429. /// <param name="profileDeceleration"></param>
  1430. public abstract bool ProfilePosition(int targetPoint, int profileVelocity, int profileAcceleration, int profileDeceleration, bool judgeTorqueLimit = true);
  1431. #endregion
  1432. #region protected abstract 子类实现方法
  1433. /// <summary>
  1434. /// 初始化Routine
  1435. /// </summary>
  1436. protected abstract void InitializeRoutine();
  1437. /// <summary>
  1438. /// 初始化参数
  1439. /// </summary>
  1440. protected abstract void InitializeParameter();
  1441. /// <summary>
  1442. /// 更新状态字
  1443. /// </summary>
  1444. /// <param name="statusWord"></param>
  1445. public abstract void UpdateStatusWord(ushort statusWord);
  1446. /// <summary>
  1447. /// 回车输入
  1448. /// </summary>
  1449. /// <param name="arg"></param>
  1450. /// <param name="value"></param>
  1451. protected abstract void AxisKeyDown(string arg, double value);
  1452. #endregion
  1453. /// <summary>
  1454. /// 监控
  1455. /// </summary>
  1456. public void Monitor()
  1457. {
  1458. }
  1459. public void Reset()
  1460. {
  1461. }
  1462. /// 停止
  1463. /// </summary>
  1464. public void Terminate()
  1465. {
  1466. }
  1467. }
  1468. }