BeckhoffCommonAxis.cs 9.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241
  1. using Aitex.Core.RT.Log;
  2. using Aitex.Core.RT.SCCore;
  3. using MECF.Framework.Common.Beckhoff.IOAxis;
  4. using MECF.Framework.Common.CommonData.PUF;
  5. using MECF.Framework.Common.TwinCat;
  6. using System;
  7. using System.Collections.Generic;
  8. using System.Linq;
  9. using System.Reflection;
  10. using System.Text;
  11. using System.Threading;
  12. using System.Threading.Tasks;
  13. using static Mono.Security.X509.X520;
  14. namespace PunkHPX8_RT.Devices.AXIS
  15. {
  16. public class BeckhoffCommonAxis
  17. {
  18. #region 常量
  19. private const string STATUS_WORD = "StatusWord";
  20. private const string DIGITAL_INPUTS = "DigitalInputs";
  21. private const string CONTROL_WORD = "ControlWord";
  22. private const string MODE_OF_OPERATION = "ModeOfOperation";
  23. protected const string PROFILE_VELOCITY = "ProfileVelocity";
  24. protected const string PROFILE_ACCEL = "ProfileAccel";
  25. protected const string PROFILE_DECEL = "ProfileDecel";
  26. private const string TARGET_POSITION = "ReferencePosition";
  27. private const string ACTUAL_VELOCITY = "Velocity";
  28. private const string ACTUAL_TORQUE = "ActualTorque";
  29. private const string POSITION_ERROR = "PositionError";
  30. private const string MOTOR_POSITION = "MotorPosition";
  31. private const string HOME_OFFSET = "HomeOffset";
  32. private const string HOMING_METHOD = "HomingMethod";
  33. protected const string HOMING_VELOCITY = "HomingVelocity";
  34. protected const string HOMING_VELOCITY_SLOW = "HomingVelocitySlow";
  35. protected const string HOMING_ACCEL = "HomingAccel";
  36. private const string NEGATIVE_TORQUE_LIMIT = "NegativeTorqueLimit";
  37. private const string POSITIVE_TORQUE_LIMIT = "PositiveTorqueLimit";
  38. private const string MANUF_STATUS = "ManufStatus";
  39. private const string SOFTWARE_LIMIT_MINUS = "SoftwareLimitMinus";
  40. private const string SOFTWARE_LIMIT_PLUS = "SoftwareLimitPlus";
  41. #endregion
  42. #region 内部变量
  43. /// <summary>
  44. /// 模块名称
  45. /// </summary>
  46. private string _module;
  47. /// <summary>
  48. /// 名称
  49. /// </summary>
  50. private string _name;
  51. /// <summary>
  52. /// 电机对象
  53. /// </summary>
  54. private JetAxisBase _axis;
  55. /// <summary>
  56. /// coe输出变量集合
  57. /// </summary>
  58. private List<string> _coeOutputs = new List<string>();
  59. /// <summary>
  60. /// coe连接状态
  61. /// </summary>
  62. private bool _coeConnected = false;
  63. /// <summary>
  64. /// beckhoff axis配置
  65. /// </summary>
  66. private BeckhoffAxis _beckhoffAxis;
  67. #endregion
  68. /// <summary>
  69. /// 初始化
  70. /// </summary>
  71. /// <param name="module"></param>
  72. /// <param name="name"></param>
  73. public BeckhoffCommonAxis(string module, string name,JetAxisBase axis)
  74. {
  75. _module = module;
  76. _name = name;
  77. _axis = axis;
  78. }
  79. /// <summary>
  80. /// 初始化COE输出变量集合
  81. /// </summary>
  82. public void InitializeCoeOutputs(BeckhoffAxis beckhoffAxis)
  83. {
  84. _beckhoffAxis= beckhoffAxis;
  85. if (beckhoffAxis != null)
  86. {
  87. foreach (BeckhoffAxisOutput item in beckhoffAxis.Outputs)
  88. {
  89. if (!_coeOutputs.Contains(item.Type) && item.Address.StartsWith("0x"))
  90. {
  91. _coeOutputs.Add(item.Type);
  92. }
  93. }
  94. }
  95. }
  96. /// <summary>
  97. /// 首次写入COE
  98. /// </summary>
  99. public void FirstStartUpWriteCOE(string variable)
  100. {
  101. if (_coeOutputs.Contains(variable))
  102. {
  103. if (!_coeConnected)
  104. {
  105. _coeConnected = true;
  106. StartUpWriteCoeThread();
  107. }
  108. }
  109. }
  110. /// <summary>
  111. /// 启动写COE变量数值线程
  112. /// </summary>
  113. public void StartUpWriteCoeThread()
  114. {
  115. bool isSimulator = SC.GetValue<bool>("System.IsSimulatorMode");
  116. if (!isSimulator)
  117. {
  118. Thread thread = new Thread(new ThreadStart(StartUpWriteCoeVariable));
  119. thread.IsBackground = true;
  120. thread.Start();
  121. }
  122. }
  123. /// <summary>
  124. /// 启动写COE变量数值
  125. /// </summary>
  126. public void StartUpWriteCoeVariable()
  127. {
  128. LOG.WriteLog(eEvent.INFO_AXIS, _module, "start write coe thread");
  129. byte homingMethod = _beckhoffAxis.HomingMethod;
  130. WriteCoeVariable(HOMING_METHOD, homingMethod);
  131. //homing accel
  132. WriteCoeVariable(HOMING_ACCEL, _axis.CalculateDivideAccelerationRatio(_beckhoffAxis.HomingAccelDecel));
  133. //homing speed home switch
  134. WriteCoeVariable(HOMING_VELOCITY, _axis.CalculateMultiplySpeedRatio(_beckhoffAxis.HomingSpeedHomeSwitch));
  135. WriteCoeVariable(HOMING_VELOCITY_SLOW, _axis.CalculateMultiplySpeedRatio(_beckhoffAxis.HomingSpeedEncoderIndex));
  136. WriteCoeVariable(HOME_OFFSET, _beckhoffAxis.HomingOffset);
  137. int negativeTorqueLimit = _beckhoffAxis.NegativeTorqueLimit;
  138. if (negativeTorqueLimit != 0)
  139. {
  140. WriteCoeVariable(NEGATIVE_TORQUE_LIMIT, negativeTorqueLimit);
  141. }
  142. int positiveTorqueLimit = _beckhoffAxis.PositiveTorqueLimit;
  143. if (positiveTorqueLimit != 0)
  144. {
  145. WriteCoeVariable(POSITIVE_TORQUE_LIMIT, positiveTorqueLimit);
  146. }
  147. int softwareForwardLimit = _beckhoffAxis.ForwardSoftwareLimit;
  148. if (softwareForwardLimit != 0)
  149. {
  150. WriteCoeVariable(SOFTWARE_LIMIT_PLUS, softwareForwardLimit);
  151. }
  152. int softReverseLimit = _beckhoffAxis.ReverseSoftwareLimit;
  153. if (softReverseLimit != 0)
  154. {
  155. WriteCoeVariable(SOFTWARE_LIMIT_MINUS, softReverseLimit);
  156. }
  157. //WriteCoeVariable(PROFILE_VELOCITY,CalculateMultiplySpeedRatio(_beckhoffAxis.Speed));
  158. //WriteCoeVariable(PROFILE_ACCEL, CalculateDivideAccelerationRatio(_beckhoffAxis.Acceleration));
  159. //WriteCoeVariable(PROFILE_DECEL,CalculateDivideAccelerationRatio(_beckhoffAxis.Deceleration));
  160. }
  161. /// <summary>
  162. /// 写COE变量数值
  163. /// </summary>
  164. /// <param name="variableName"></param>
  165. /// <param name="value"></param>
  166. private void WriteCoeVariable(string variableName, object value)
  167. {
  168. string str = $"{_module}.{_name}";
  169. if (_coeOutputs.Contains(variableName))
  170. {
  171. bool result = TwincatCoeManager.Instance.WriteVariableValue(str, variableName, value);
  172. if (result)
  173. {
  174. LOG.WriteLog(eEvent.INFO_AXIS, str, $"coe variable {variableName} value {value} success");
  175. }
  176. }
  177. }
  178. /// <summary>
  179. /// 订阅变量
  180. /// </summary>
  181. public void SubscriptionVariable()
  182. {
  183. _axis.AxisSubscribeUpdateVariable(STATUS_WORD);
  184. _axis.AxisSubscribeUpdateVariable(DIGITAL_INPUTS);
  185. _axis.AxisSubscribeUpdateVariable(CONTROL_WORD);
  186. _axis.AxisSubscribeUpdateVariable(MODE_OF_OPERATION);
  187. _axis.AxisSubscribeUpdateVariable(PROFILE_VELOCITY);
  188. _axis.AxisSubscribeUpdateVariable(PROFILE_ACCEL);
  189. _axis.AxisSubscribeUpdateVariable(PROFILE_DECEL);
  190. _axis.AxisSubscribeUpdateVariable(TARGET_POSITION);
  191. _axis.AxisSubscribeUpdateVariable(MOTOR_POSITION);
  192. _axis.AxisSubscribeUpdateVariable(POSITION_ERROR);
  193. _axis.AxisSubscribeUpdateVariable(ACTUAL_TORQUE);
  194. _axis.AxisSubscribeUpdateVariable(ACTUAL_VELOCITY);
  195. _axis.AxisSubscribeUpdateVariable(HOME_OFFSET);
  196. _axis.AxisSubscribeUpdateVariable(HOMING_METHOD);
  197. _axis.AxisSubscribeUpdateVariable(HOMING_VELOCITY);
  198. _axis.AxisSubscribeUpdateVariable(HOMING_VELOCITY_SLOW);
  199. _axis.AxisSubscribeUpdateVariable(POSITIVE_TORQUE_LIMIT);
  200. _axis.AxisSubscribeUpdateVariable(NEGATIVE_TORQUE_LIMIT);
  201. _axis.AxisSubscribeUpdateVariable(HOMING_ACCEL);
  202. _axis.AxisSubscribeUpdateVariable(MANUF_STATUS);
  203. }
  204. /// <summary>
  205. /// 写入控制字
  206. /// </summary>
  207. /// <param name="controlWord"></param>
  208. public bool WriteControlWord(ushort controlWord)
  209. {
  210. return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{CONTROL_WORD}", controlWord);
  211. }
  212. /// <summary>
  213. /// 写入ModeOfOperation
  214. /// </summary>
  215. /// <param name="modeOfOperation"></param>
  216. /// <returns></returns>
  217. public bool WriteModeOfMode(AxisModeOfOperation modeOfOperation)
  218. {
  219. return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{MODE_OF_OPERATION}", (byte)modeOfOperation);
  220. }
  221. /// <summary>
  222. /// 写变量数值
  223. /// </summary>
  224. /// <param name="variableName"></param>
  225. /// <param name="value"></param>
  226. /// <returns></returns>
  227. public bool WriteVariable(string variableName, object value)
  228. {
  229. return BeckhoffAxisManager.Instance.WriteVariableValue($"{_module}.{_name}.{variableName}", value);
  230. }
  231. }
  232. }