YaskawaProfilePositionRoutine.cs 6.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170
  1. using Aitex.Core.RT.Log;
  2. using Aitex.Core.RT.Routine;
  3. using MECF.Framework.Common.CommonData.PUF;
  4. using MECF.Framework.Common.Routine;
  5. using MECF.Framework.Common.TwinCat;
  6. using PunkHPX8_Core;
  7. using System;
  8. using System.Collections.Generic;
  9. using System.Linq;
  10. using System.Text;
  11. using System.Threading.Tasks;
  12. namespace PunkHPX8_RT.Devices.AXIS.Yaskawa
  13. {
  14. public class YaskawaProfilePositionRoutine : RoutineBase, IRoutine
  15. {
  16. private enum ProfilePositionStep
  17. {
  18. WriteInitControlWord,
  19. ProfileModeOfOperation,
  20. WriteProfileParameter,
  21. WriteProfilePositionControlWord,
  22. Delay,
  23. CheckRun,
  24. NoneModeOfOperation,
  25. EnableOperation,
  26. End
  27. }
  28. #region 常量
  29. private const string PROFILE_VELOCITY = "ProfileVelocity";
  30. private const string PROFILE_ACCEL = "ProfileAccel";
  31. private const string PROFILE_DECEL = "ProfileDecel";
  32. private const string TARGET_POSITION = "TargetPosition";
  33. #endregion
  34. #region 内部变量
  35. private JetAxisBase _axis;
  36. private int _profileVelocity = 0;
  37. private int _profileAcceleration = 0;
  38. private int _profileDeceleration = 0;
  39. private int _targetPosition;
  40. private bool _judgeTorqueLimit = false;
  41. private BeckhoffCommonAxis _beckhoffCommonAxis;
  42. #endregion
  43. public YaskawaProfilePositionRoutine(string module,JetAxisBase axis, BeckhoffCommonAxis beckhoffCommonAxis) : base(module)
  44. {
  45. _axis = axis;
  46. _beckhoffCommonAxis = beckhoffCommonAxis;
  47. }
  48. public void Abort()
  49. {
  50. Runner.Stop("Manual Abort");
  51. }
  52. public RState Monitor()
  53. {
  54. Runner.Run(ProfilePositionStep.WriteInitControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); },() => { return CheckControlWord(0x0F); },500)
  55. .Run(ProfilePositionStep.ProfileModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.ProfilePositionMode); },()=> { return CheckModeOfOperation((byte)AxisModeOfOperation.ProfilePositionMode); }, 1000)
  56. .Run(ProfilePositionStep.WriteProfileParameter, () => { return WriteProfileParameter(); },NullFun,100)
  57. .Run(ProfilePositionStep.WriteProfilePositionControlWord, () => { return _beckhoffCommonAxis.WriteControlWord(0x3F); }, () => { return CheckControlWord(0x3F); },1000)
  58. .Delay(ProfilePositionStep.Delay,1000)
  59. .WaitWithStopCondition(ProfilePositionStep.CheckRun,CheckRun,CheckRunStop)
  60. .Run(ProfilePositionStep.NoneModeOfOperation, () => { return _beckhoffCommonAxis.WriteModeOfMode(AxisModeOfOperation.None); },() => { return CheckModeOfOperation((byte)AxisModeOfOperation.None); },1000)
  61. .Run(ProfilePositionStep.EnableOperation, () => { return _beckhoffCommonAxis.WriteControlWord(0x0F); }, () => { return CheckControlWord(0x0F); },1000)
  62. .End(ProfilePositionStep.End,NullFun,100);
  63. return Runner.Status;
  64. }
  65. private bool WriteProfileParameter()
  66. {
  67. bool result= BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_VELOCITY}", _profileVelocity);
  68. if(!result)
  69. {
  70. return false;
  71. }
  72. result=BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_ACCEL}", _profileAcceleration);
  73. if(!result)
  74. {
  75. return false;
  76. }
  77. result=BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{PROFILE_DECEL}", _profileDeceleration);
  78. if(!result)
  79. {
  80. return false;
  81. }
  82. result= BeckhoffAxisManager.Instance.WriteVariableValue($"{Module}.{TARGET_POSITION}", _targetPosition);
  83. return result;
  84. }
  85. public bool CheckModeOfOperation(byte modeOfOperation)
  86. {
  87. return _axis.ModeOfOperation == modeOfOperation;
  88. }
  89. public bool CheckControlWord(ushort controlWord)
  90. {
  91. return _axis.ControlWord == controlWord;
  92. }
  93. /// <summary>
  94. /// 检验是否正常运行
  95. /// </summary>
  96. /// <returns></returns>
  97. private bool CheckRun()
  98. {
  99. if (_judgeTorqueLimit)
  100. {
  101. return _axis.InTargetPosition && !_axis.IsRun && _axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
  102. }
  103. else
  104. {
  105. bool isTorqueOver = TorqueIsOverLimit();
  106. //过载同时掉电
  107. if (isTorqueOver && !_axis.IsSwitchOn)
  108. {
  109. return true;
  110. }
  111. if(_axis.InTargetPosition&&_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition))
  112. {
  113. return true;
  114. }
  115. return false;
  116. }
  117. }
  118. /// <summary>
  119. /// Torque是否过载
  120. /// </summary>
  121. /// <returns></returns>
  122. private bool TorqueIsOverLimit()
  123. {
  124. return !_axis.MotionData.TorqueLimited;
  125. }
  126. /// <summary>
  127. /// 检验是否停止
  128. /// </summary>
  129. /// <returns></returns>
  130. private bool CheckRunStop()
  131. {
  132. //没有运动
  133. bool isStop = !_axis.IsRun&&!_axis.JudgeCurrentPositionIsInTargetPosition(_targetPosition);
  134. if (isStop&&_judgeTorqueLimit)
  135. {
  136. ErrorMsg = "position is stop";
  137. return true;
  138. }
  139. if (_judgeTorqueLimit)
  140. {
  141. if (TorqueIsOverLimit())
  142. {
  143. ErrorMsg = "Torque is over limit";
  144. return true;
  145. }
  146. }
  147. return false;
  148. }
  149. public RState Start(params object[] objs)
  150. {
  151. _targetPosition = (int)objs[0];
  152. _profileVelocity = (int)objs[1];
  153. _profileAcceleration = (int)objs[2];
  154. _profileDeceleration = (int)objs[3];
  155. if(objs.Length>=5)
  156. {
  157. _judgeTorqueLimit = (bool)objs[4];
  158. }
  159. return Runner.Start(Module, "ProfilePosition");
  160. }
  161. }
  162. }