GalilLipselAxis.cs 15 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391
  1. using Aitex.Core.RT.DataCenter;
  2. using Aitex.Core.RT.Event;
  3. using Aitex.Core.RT.Fsm;
  4. using Aitex.Core.RT.Log;
  5. using Aitex.Core.RT.Routine;
  6. using Aitex.Core.RT.SCCore;
  7. using Aitex.Core.Util;
  8. using MECF.Framework.Common.CommonData.PUF;
  9. using MECF.Framework.Common.Device.Galil;
  10. using MECF.Framework.Common.Device;
  11. using MECF.Framework.Common.TwinCat;
  12. using PunkHPX8_Core;
  13. using PunkHPX8_RT.Modules;
  14. using System;
  15. using System.Collections.Generic;
  16. using System.Data;
  17. using System.Linq;
  18. using System.Reflection;
  19. using System.Text;
  20. using System.Threading.Tasks;
  21. using static Mono.Security.X509.X520;
  22. namespace PunkHPX8_RT.Devices.AXIS.GalilLipsel
  23. {
  24. public class GalilLipselAxis : JetAxisBase
  25. {
  26. #region 内部变量
  27. /// <summary>
  28. /// Home routine
  29. /// </summary>
  30. private GalilLipselHomeRoutine _homeRoutine;
  31. /// <summary>
  32. /// 运动Routine
  33. /// </summary>
  34. private GalilLipselProfilePositionRoutine _profilePositionRoutine;
  35. /// <summary>
  36. /// Switch On Routine
  37. /// </summary>
  38. private GalilLipselSwitchOnRoutine _switchOnRoutine;
  39. /// <summary>
  40. /// Switch Off Routine
  41. /// </summary>
  42. private GalilLipselSwitchOffRoutine _switchOffRoutine;
  43. /// <summary>
  44. /// Stop Position
  45. /// </summary>
  46. private GalilLipselStopPositionRoutine _stopPositionRoutine;
  47. /// <summary>
  48. /// Galil共用对象
  49. /// </summary>
  50. private GalilCommonAxis _galilCommoAxis;
  51. #endregion
  52. /// <summary>
  53. /// 构造函数
  54. /// </summary>
  55. /// <param name="Module"></param>
  56. public GalilLipselAxis(string module, string name) : base(module, name)
  57. {
  58. }
  59. /// <summary>
  60. /// 初始化参数
  61. /// </summary>
  62. protected override void InitializeParameter()
  63. {
  64. _accelerationRatio = 1;
  65. _speedRatio = 1;
  66. _galilCommoAxis = new GalilCommonAxis(Module, Name, this);
  67. }
  68. /// <summary>
  69. /// 初始化Routine
  70. /// </summary>
  71. protected override void InitializeRoutine()
  72. {
  73. _homeRoutine = new GalilLipselHomeRoutine($"{Module}.{Name}", this,_galilCommoAxis);
  74. _profilePositionRoutine = new GalilLipselProfilePositionRoutine($"{Module}.{Name}",this, _galilCommoAxis);
  75. _switchOnRoutine = new GalilLipselSwitchOnRoutine($"{Module}.{Name}",this, _galilCommoAxis);
  76. _switchOffRoutine = new GalilLipselSwitchOffRoutine($"{Module}.{Name}", this,_galilCommoAxis);
  77. _stopPositionRoutine = new GalilLipselStopPositionRoutine($"{Module}.{Name}", this,_galilCommoAxis);
  78. }
  79. /// <summary>
  80. /// 中止操作
  81. /// </summary>
  82. /// <param name="cmd"></param>
  83. /// <param name="args"></param>
  84. /// <returns></returns>
  85. public override bool StopPositionOperation()
  86. {
  87. lock (_operationLocker)
  88. {
  89. if (_currentOperation == MotionOperation.Position)
  90. {
  91. _profilePositionRoutine.Abort();
  92. _status = _stopPositionRoutine.Start();
  93. _currentOperation = MotionOperation.StopPosition;
  94. }
  95. else if (_currentOperation == MotionOperation.Home)
  96. {
  97. _homeRoutine.Abort();
  98. _status = _homeRoutine.Start();
  99. _currentOperation = MotionOperation.StopPosition;
  100. }
  101. else
  102. {
  103. EndOperation();
  104. }
  105. }
  106. return true;
  107. }
  108. /// <summary>
  109. /// 更新StatusWord
  110. /// </summary>
  111. /// <param name="status"></param>
  112. public override void UpdateStatusWord(ushort status)
  113. {
  114. }
  115. /// <summary>
  116. /// Home
  117. /// </summary>
  118. public override bool Home()
  119. {
  120. bool result = base.Home();
  121. if (!result)
  122. {
  123. return false;
  124. }
  125. MotionData.IsHomed = false;
  126. _homeRoutine.Start(_homeTimeout, _axisConfig.HomingAcceleration,_axisConfig.HomingDeceleration,_axisConfig.HomingSpeed,_axisConfig.HomingOffset);
  127. IsHomeSwitchedTriggered = false;
  128. return true;
  129. }
  130. /// <summary>
  131. /// 停止
  132. /// </summary>
  133. public override void Stop()
  134. {
  135. }
  136. /// <summary>
  137. /// SwitchOff
  138. /// </summary>
  139. public override bool SwitchOff()
  140. {
  141. if (_status == RState.Running)
  142. {
  143. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot switchoff");
  144. return false;
  145. }
  146. _currentOperation = MotionOperation.SwitchOff;
  147. _switchOffRoutine.Start();
  148. _status = RState.Running;
  149. return true;
  150. }
  151. /// <summary>
  152. /// SwitchOn
  153. /// </summary>
  154. public override bool SwitchOn()
  155. {
  156. if (_status == RState.Running)
  157. {
  158. EV.PostAlarmLog($"{Module}.{Name}", eEvent.ERR_AXIS, $"{Module}.{Name} current execute {_currentOperation},cannot SwitchOn");
  159. return false;
  160. }
  161. _currentOperation = MotionOperation.SwitchOn;
  162. _switchOnRoutine.Start();
  163. _status = RState.Running;
  164. return true;
  165. }
  166. /// <summary>
  167. /// 开始运动
  168. /// </summary>
  169. /// <returns></returns>
  170. public override bool StartMotion()
  171. {
  172. return _galilCommoAxis.WriteStartMotion();
  173. }
  174. /// <summary>
  175. /// 定时器
  176. /// </summary>
  177. /// <returns></returns>
  178. public override bool OnTimer()
  179. {
  180. lock (_operationLocker)
  181. {
  182. if (_status == RState.Running)
  183. {
  184. if (_currentOperation == MotionOperation.Position)
  185. {
  186. RState state = _profilePositionRoutine.Monitor();
  187. if (state == RState.End)
  188. {
  189. _inTargetPosition = false;
  190. EndOperation();
  191. _status = RState.End;
  192. LOG.WriteLog(eEvent.INFO_AXIS, $"{Module}.{Name}", $"Position Complete,Current Position {MotionData.MotorPosition}");
  193. }
  194. else if (state == RState.Failed || state == RState.Timeout)
  195. {
  196. if (_currentOperation == MotionOperation.Position)
  197. {
  198. _inTargetPosition = false;
  199. EndOperation();
  200. _status = RState.Failed;
  201. LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", $"Profile Position error {_profilePositionRoutine.ErrorMsg}");
  202. }
  203. }
  204. }
  205. else if (_currentOperation == MotionOperation.Home)
  206. {
  207. RState state = _homeRoutine.Monitor();
  208. if (state == RState.End)
  209. {
  210. MotionData.IsHomed = true;
  211. IsHomed = true;
  212. EndOperation();
  213. _status = RState.End;
  214. }
  215. else if (state == RState.Failed || state == RState.Timeout)
  216. {
  217. EndOperation();
  218. _status = RState.Failed;
  219. LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Home error");
  220. }
  221. }
  222. else if (_currentOperation == MotionOperation.SwitchOn)
  223. {
  224. RState state = _switchOnRoutine.Monitor();
  225. if (state == RState.End)
  226. {
  227. EndOperation();
  228. _status = RState.End;
  229. }
  230. else if (state == RState.Failed || state == RState.Timeout)
  231. {
  232. EndOperation();
  233. _status = RState.Failed;
  234. LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch On error");
  235. }
  236. }
  237. else if (_currentOperation == MotionOperation.SwitchOff)
  238. {
  239. RState state = _switchOffRoutine.Monitor();
  240. if (state == RState.End)
  241. {
  242. EndOperation();
  243. _status = RState.End;
  244. }
  245. else if (state == RState.Failed || state == RState.Timeout)
  246. {
  247. EndOperation();
  248. _status = RState.Failed;
  249. LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "Switch Off error");
  250. }
  251. }
  252. else if (_currentOperation == MotionOperation.StopPosition)
  253. {
  254. RState state = _stopPositionRoutine.Monitor();
  255. if (state == RState.End)
  256. {
  257. EndOperation();
  258. _status = RState.End;
  259. }
  260. else if (state == RState.Failed || state == RState.Timeout)
  261. {
  262. EndOperation();
  263. _status = RState.Failed;
  264. LOG.WriteLog(eEvent.ERR_AXIS, $"{Module}.{Name}", "StopPosition error");
  265. }
  266. }
  267. }
  268. }
  269. JudgeRunMonitor();
  270. return true;
  271. }
  272. /// <summary>
  273. /// 位置
  274. /// </summary>
  275. /// <param name="targetPoint"></param>
  276. public override bool ProfilePosition(int targetPoint, int profileVelocity, int profileAcceleration, int profileDeceleration, bool judgeTorqueLimit = true)
  277. {
  278. if (_status == RState.Running)
  279. {
  280. LOG.WriteLog(eEvent.WARN_AXIS, $"{Module}.{Name}", $"current execute {_currentOperation},cannot profile position");
  281. return false;
  282. }
  283. if (profileAcceleration == 0)
  284. {
  285. profileAcceleration = _profileAcceleration;
  286. }
  287. if (profileDeceleration == 0)
  288. {
  289. profileDeceleration = _profileDeceleration;
  290. }
  291. _status = _profilePositionRoutine.Start(targetPoint, profileVelocity, profileAcceleration, profileDeceleration, judgeTorqueLimit);
  292. _currentOperation = MotionOperation.Position;
  293. _inTargetPosition = false;
  294. return true;
  295. }
  296. /// <summary>
  297. /// 改变速度
  298. /// </summary>
  299. /// <param name="speed"></param>
  300. /// <returns></returns>
  301. public override bool ChangeSpeed(int speed)
  302. {
  303. return _galilCommoAxis.WriteSpeed(speed);
  304. }
  305. /// <summary>
  306. /// 改变速度加速度
  307. /// </summary>
  308. /// <param name="speed"></param>
  309. /// <returns></returns>
  310. public override bool ChangeSpeedAcceleration(int speed, int acceleration, int deceleration)
  311. {
  312. bool result = _galilCommoAxis.WriteSpeed(speed);
  313. if (result)
  314. {
  315. result=_galilCommoAxis.WriteAcceleration(acceleration);
  316. if (result)
  317. {
  318. result = _galilCommoAxis.WriteDeceleration(deceleration);
  319. }
  320. }
  321. return result;
  322. }
  323. /// <summary>
  324. /// KeyDown事件
  325. /// </summary>
  326. /// <param name="arg"></param>
  327. /// <param name="value"></param>
  328. protected override void AxisKeyDown(string arg, double value)
  329. {
  330. switch (arg)
  331. {
  332. case PROFILE_VELOCITY:
  333. _profileVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
  334. _commandMotionData.FileProfileVelocity = value;
  335. BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileVelocity);
  336. break;
  337. case PROFILE_ACCEL:
  338. _profileAcceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
  339. _commandMotionData.FileAcceleration = value;
  340. BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileAcceleration);
  341. break;
  342. case PROFILE_DECEL:
  343. _profileDeceleration = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
  344. _commandMotionData.FileDeceleration = value;
  345. BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{Name}.{arg}", _profileDeceleration);
  346. break;
  347. case HOMING_VELOCITY:
  348. _profileHomingVelocity = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
  349. _commandMotionData.FileHomingVelocity = value;
  350. TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocity);
  351. break;
  352. case HOMING_VELOCITY_SLOW:
  353. _profileHomingVelocitySlow = CalculateMultiplySpeedRatio(CalculateValueMultiplyScale(value));
  354. _commandMotionData.FileHomingVelocitySlow = value;
  355. TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingVelocitySlow);
  356. break;
  357. case HOMING_ACCEL:
  358. _profileHomingAccel = CalculateDivideAccelerationRatio(CalculateValueMultiplyScale(value));
  359. _commandMotionData.FileHomingAccel = value;
  360. TwincatCoeManager.Instance.WriteVariableValue($"{Module}.{Name}", arg, _profileHomingAccel);
  361. break;
  362. }
  363. }
  364. /// <summary>
  365. /// 初始化Axis配置
  366. /// </summary>
  367. /// <returns></returns>
  368. protected override AxisConfig InitializeAxisConfig()
  369. {
  370. return GalilControllerCfgManager.Instance.GetGalilAxisConfig(Module.ToString(), Name);
  371. }
  372. /// <summary>
  373. /// 订阅变量
  374. /// </summary>
  375. protected override void SubscriptionVariable()
  376. {
  377. _galilCommoAxis.SubscriptionVariable();
  378. }
  379. }
  380. }