WagoSocketSimulator.cs 21 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515
  1. using Aitex.Common.Util;
  2. using Aitex.Core.RT.Device;
  3. using Aitex.Core.RT.Log;
  4. using Aitex.Core.Util;
  5. using MECF.Framework.Common.Device.Festo;
  6. using MECF.Framework.Common.Device.Wago;
  7. using MECF.Framework.Common.Net;
  8. using MECF.Framework.Common.Simulator;
  9. using MECF.Framework.Simulator.Core.Driver;
  10. using System;
  11. using System.Collections.Generic;
  12. using System.IO;
  13. namespace CyberX8_Simulator.Devices
  14. {
  15. public class WagoSocketSimulator : SocketDeviceSimulator
  16. {
  17. private const short WRITE_DO_STARTADDRESS = 0x0200;
  18. private const short WRITE_AO_STARTADDRESS = 0x0200;
  19. //键是名字,值是对应数据所在的位置 注意:要和WagoControlCfg里面的地址顺序对上
  20. public Dictionary<string, int> DONameIndexDic;
  21. public Dictionary<string, int> DINameIndexDic;
  22. public Dictionary<string, int> AINameIndexDic;
  23. public Dictionary<string, int> AONameIndexDic;
  24. private IByteTransform byteTransform = new BigEndianByteTransformBase();
  25. //存储模拟器数据的数组
  26. public byte[] DOBytes = new byte[100];
  27. public short[] AOShorts = new short[100];
  28. public byte[] DIBytes = new byte[100];
  29. public short[] AIShorts = new short[100];
  30. /// <summary>
  31. /// 写DO锁
  32. /// </summary>
  33. private object _writeDOLocker = new object();
  34. /// <summary>
  35. /// 写AO锁
  36. /// </summary>
  37. private object _writeAOLocker = new object();
  38. /// <summary>
  39. /// Festo Data Buffer
  40. /// </summary>
  41. private Dictionary<string, bool> _festoDataBuffer = new Dictionary<string, bool>();
  42. /// <summary>
  43. /// 定时器
  44. /// </summary>
  45. private PeriodicJob _periodicJob;
  46. public WagoSocketSimulator(int port):base(port)
  47. {
  48. SimulatorCommManager.Instance.OnUpdateVariableValueChanged += UpdataDataCausedByOtherModule;
  49. MotorSimulator.Instance.OnUpdateWagoDatasChanged += UpdataDataCausedByOtherModule;
  50. InitializeData(port);
  51. }
  52. private void UpdataDataCausedByOtherModule(string name, bool value, bool invert)
  53. {
  54. value = invert ? !value : value;
  55. //AI Data
  56. if (AINameIndexDic.ContainsKey(name))
  57. {
  58. switch (name)
  59. {
  60. case "r_LoaderA_LS_Vacuum_anlg":
  61. case "r_LoaderB_LS_Vacuum_anlg":
  62. AIShorts[AINameIndexDic[name]] = value ? (short)0x32C8 : (short)0x2AF8;
  63. break;
  64. case "r_DPUF_A_CHUCK_A_VAC":
  65. case "r_DPUF_A_CHUCK_B_VAC":
  66. AIShorts[AINameIndexDic[name]] = value ? (short)0x0C80 : (short)0x32C8;
  67. break;
  68. case "r_LOADERA_BERNOULLI_PRESSURE":
  69. case "r_LOADERB_BERNOULLI_PRESSURE":
  70. case "r_LOADERA_CHUCK_BLADDER":
  71. case "r_LOADERB_CHUCK_BLADDER":
  72. case "r_LOADERA_WS_BLADDER_PRESSURE":
  73. case "r_LOADERB_WS_BLADDER_PRESSURE":
  74. AIShorts[AINameIndexDic[name]] = value ? (short)0x2AF8 : (short)0x00;
  75. break;
  76. default:
  77. break;
  78. }
  79. }
  80. //DI Data
  81. UpdataDIBytes(name, value ? 1 : 0);
  82. //Festo Data
  83. if (name == "FlowTestClamp") _festoDataBuffer[name] = value;
  84. }
  85. /// <summary>
  86. /// 触发Wago对应数据更新
  87. /// </summary>
  88. /// <param name="position"></param>
  89. /// <param name="value"></param>
  90. private void UpdateDataCausedByWago(int position, bool value)
  91. {
  92. if (position == DONameIndexDic["c_PUF_CHUCK"])
  93. {
  94. UpdataDIBytes("r_PUF_A_CHUCK_OUT", value ? 1 : 0);
  95. UpdataDIBytes("r_PUF_B_CHUCK_OUT", value ? 1 : 0);
  96. UpdataDIBytes("r_PUF_A_CHUCK_IN", !value ? 1 : 0);
  97. UpdataDIBytes("r_PUF_B_CHUCK_IN", !value ? 1 : 0);
  98. }
  99. }
  100. /// <summary>
  101. /// 初始化字典
  102. /// </summary>
  103. private void InitializeData(int port) //端口用于初始化不同Wago设备的字典
  104. {
  105. //加载对应配置文件 WagoControllerCfg-Simulator.xml
  106. try
  107. {
  108. string oldXmlPath = PathManager.GetCfgDir();
  109. string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\WagoControllerCfg-Simulator.xml";
  110. WagoControllerCfg cfg = CustomXmlSerializer.Deserialize<WagoControllerCfg>(new FileInfo(newXmlPath));
  111. if (cfg != null)
  112. {
  113. foreach (WagoDeviceConfig config in cfg.WagoDeviceConfigs)
  114. {
  115. if (port == config.Port)
  116. {
  117. if(config.Module == "Loader" && _periodicJob == null)
  118. {
  119. _periodicJob = new PeriodicJob(100, OnTimer, $"Wago {config.Module} timer",true);
  120. }
  121. //加载DO
  122. int i = 0;
  123. DONameIndexDic = new Dictionary<string, int>();
  124. foreach (var group in config.WagoDigOut.WagoDOGroups)
  125. {
  126. foreach (var item in group.WagoDOs)
  127. {
  128. DONameIndexDic[item.Name] = i;
  129. i++;
  130. }
  131. }
  132. //加载DI
  133. i = 0;
  134. DINameIndexDic = new Dictionary<string, int>();
  135. foreach (var group in config.WagoDigIn.WagoDIGroups)
  136. {
  137. foreach(var item in group.WagoDIs)
  138. {
  139. DINameIndexDic[item.Name] = i;
  140. i++;
  141. }
  142. }
  143. //加载AO
  144. i = 0;
  145. AONameIndexDic = new Dictionary<string, int>();
  146. foreach (var group in config.WagoAnoOut.WagoAOGroups)
  147. {
  148. foreach (var item in group.WagoAOs)
  149. {
  150. AONameIndexDic[item.Name] = i;
  151. i++;
  152. }
  153. }
  154. //加载AI
  155. i = 0;
  156. AINameIndexDic = new Dictionary<string, int>();
  157. foreach (var group in config.WagoAnoIn.WagoAIGroups)
  158. {
  159. foreach (var item in group.WagoAIs)
  160. {
  161. AINameIndexDic[item.Name] = i;
  162. i++;
  163. }
  164. }
  165. }
  166. }
  167. }
  168. }
  169. catch
  170. {
  171. LOG.WriteLog(eEvent.ERR_WAGO, "Wago", "Load wago WagoControllerCfg-Simulator.xml failed");
  172. }
  173. //设置IO变量默认值
  174. if (AINameIndexDic.ContainsKey("r_LoaderA_LS_Vacuum_anlg")) AIShorts[AINameIndexDic["r_LoaderA_LS_Vacuum_anlg"]] = 0x32C8;
  175. if (AINameIndexDic.ContainsKey("r_LoaderB_LS_Vacuum_anlg")) AIShorts[AINameIndexDic["r_LoaderB_LS_Vacuum_anlg"]] = 0x32C8;
  176. if (AINameIndexDic.ContainsKey("r_DPUF_A_CHUCK_A_VAC")) AIShorts[AINameIndexDic["r_DPUF_A_CHUCK_A_VAC"]] = 0x32C8;
  177. if (AINameIndexDic.ContainsKey("r_DPUF_A_CHUCK_B_VAC")) AIShorts[AINameIndexDic["r_DPUF_A_CHUCK_B_VAC"]] = 0x32C8;
  178. }
  179. #region 公共方法
  180. public void UpdataDOBytes(string name,int value)
  181. {
  182. if (DONameIndexDic.ContainsKey(name))
  183. {
  184. if (DONameIndexDic[name] < DOBytes.Length)
  185. {
  186. DOBytes[DONameIndexDic[name]] = value == 0 ? (byte)0 : (byte)1;
  187. }
  188. }
  189. }
  190. public void UpdataDIBytes(string name, int value)
  191. {
  192. if (DINameIndexDic.ContainsKey(name))
  193. {
  194. if (DINameIndexDic[name] < DIBytes.Length)
  195. {
  196. DIBytes[DINameIndexDic[name]] = value == 0 ? (byte)0 : (byte)1;
  197. }
  198. }
  199. }
  200. public void UpdataAOShorts(string name, int value)
  201. {
  202. if (AONameIndexDic.ContainsKey(name))
  203. {
  204. string hexValue = value.ToString("X2");
  205. try
  206. {
  207. short result = Convert.ToInt16(hexValue, 16);
  208. if (AONameIndexDic[name] < AOShorts.Length)
  209. {
  210. AOShorts[AONameIndexDic[name]] = result;
  211. }
  212. }
  213. catch (FormatException)
  214. {
  215. }
  216. }
  217. }
  218. public void UpdataAIShorts(string name, int value)
  219. {
  220. if (AINameIndexDic.ContainsKey(name))
  221. {
  222. string hexValue = value.ToString("X2");
  223. try
  224. {
  225. short result = Convert.ToInt16(hexValue, 16);
  226. if(AINameIndexDic[name] < AIShorts.Length)
  227. {
  228. AIShorts[AINameIndexDic[name]] = result;
  229. }
  230. }
  231. catch (FormatException)
  232. {
  233. }
  234. }
  235. }
  236. #endregion
  237. #region 功能方法
  238. /// <summary>
  239. /// 将长度为8的二进制byte数组转成对应十六进制byte值(大端模式)
  240. /// </summary>
  241. /// <param name="byteArray"></param>
  242. /// <returns></returns>
  243. public byte ConvertByteArrayToHex(byte[] byteArray)
  244. {
  245. byte result = 0;
  246. // 先将 byte 数组转换为二进制数
  247. int binaryValue = 0;
  248. for (int i = 0; i < 8; i++)
  249. {
  250. binaryValue |= (byteArray[i] << (7 - i));
  251. }
  252. // 逆转二进制数
  253. int reversedValue = 0;
  254. for (int i = 0; i < 8; i++)
  255. {
  256. reversedValue |= ((binaryValue >> i) & 1) << (7 - i);
  257. }
  258. // 转换为十六进制byte
  259. if (byte.TryParse(reversedValue.ToString("X2"), System.Globalization.NumberStyles.HexNumber, null, out result))
  260. {
  261. return result;
  262. }
  263. return 0;
  264. }
  265. /// <summary>
  266. /// 将short数组转成长度两倍的byte数组
  267. /// </summary>
  268. /// <param name="shortArray"></param>
  269. /// <returns></returns>
  270. private byte[] ConvertShortArrayToByteArray(short[] shortArray)
  271. {
  272. byte[] byteArray = new byte[shortArray.Length * 2];
  273. for (int i = 0; i < shortArray.Length; i++)
  274. {
  275. byte[] tempBytes = BitConverter.GetBytes(shortArray[i]);
  276. Array.Reverse(tempBytes);
  277. Array.Copy(tempBytes, 0, byteArray, i * 2, 2);
  278. }
  279. return byteArray;
  280. }
  281. #endregion
  282. protected override void ProcessUnsplitMessage(byte[] data)
  283. {
  284. byte command = data[7];
  285. if (command == 0x01) //读DO
  286. {
  287. short flag = byteTransform.TransInt16(data, 0);
  288. byte channel = data[6];
  289. short startAddress = byteTransform.TransInt16(data, 8);
  290. short bitCount = byteTransform.TransInt16(data, 10);
  291. byte byteCount = (byte)(bitCount / 8 + 1);
  292. byte[] bytes = new byte[byteCount];
  293. for(int i = 0; i < byteCount;i++)
  294. {
  295. byte[] tempbytes = new byte[8];
  296. Array.Copy(DOBytes,8 * i, tempbytes, 0, 8);
  297. bytes[i] = ConvertByteArrayToHex(tempbytes);
  298. }
  299. OnWriteMessage(CreateReadDigitalResponse(flag, channel, command, byteCount, bytes));
  300. return;
  301. }
  302. else if(command == 0x03)//读AO
  303. {
  304. short flag = byteTransform.TransInt16(data, 0);
  305. byte channel = data[6];
  306. short startAddress = byteTransform.TransInt16(data, 8);
  307. short registerCount = byteTransform.TransInt16(data, 10);
  308. short[] shorts = new short[registerCount];//获取指定寄存器里的内容
  309. Array.Copy(AOShorts, 0, shorts, 0, registerCount);
  310. byte[] bytes = new byte[registerCount * 2];
  311. bytes = ConvertShortArrayToByteArray(shorts); //转入长度为shorts数组长度两倍的bytes数组中
  312. OnWriteMessage(CreateReadAnalogyResponse(flag, channel, command, (byte)registerCount, bytes));
  313. return;
  314. }
  315. else if (command == 0x02)//读DI
  316. {
  317. short flag = byteTransform.TransInt16(data, 0);
  318. byte channel = data[6];
  319. short startAddress = byteTransform.TransInt16(data, 8);
  320. short bitCount = byteTransform.TransInt16(data, 10);
  321. byte byteCount = (byte)(bitCount / 8 + 1);
  322. byte[] bytes = new byte[byteCount];
  323. for (int i = 0; i < byteCount; i++)
  324. {
  325. byte[] tempbytes = new byte[8];
  326. Array.Copy(DIBytes, 8 * i, tempbytes, 0, 8);
  327. bytes[i] = ConvertByteArrayToHex(tempbytes);
  328. }
  329. OnWriteMessage(CreateReadDigitalResponse(flag, channel, command, byteCount, bytes));
  330. return;
  331. }
  332. else if (command == 0x04)//读AI
  333. {
  334. short flag = byteTransform.TransInt16(data, 0);
  335. byte channel = data[6];
  336. short startAddress = byteTransform.TransInt16(data, 8);
  337. short registerCount = byteTransform.TransInt16(data, 10);
  338. short[] shorts = new short[registerCount];//获取指定寄存器里的内容
  339. Array.Copy(AIShorts, 0, shorts, 0, registerCount);
  340. byte[] bytes = new byte[registerCount * 2];
  341. bytes = ConvertShortArrayToByteArray(shorts); //转入长度为shorts数组两倍的bytes数组中
  342. OnWriteMessage(CreateReadAnalogyResponse(flag, channel, command, (byte)registerCount, bytes));
  343. return;
  344. }
  345. else if (command == 0x05)//写DO
  346. {
  347. short startAddress = byteTransform.TransInt16(data, 8);
  348. if (startAddress > 0x03FF || startAddress < WRITE_DO_STARTADDRESS)
  349. {
  350. short flag = byteTransform.TransInt16(data, 0);
  351. byte channel = data[6];
  352. OnWriteMessage(CreateError(flag, channel, command, 0x02)); //地址错误
  353. return;
  354. }
  355. int position = startAddress - WRITE_DO_STARTADDRESS;
  356. bool status = data[10] == 0xFF ? true : false;
  357. lock (_writeDOLocker)
  358. {
  359. DOBytes[position] = status ? (byte)1 : (byte)0;
  360. }
  361. OnWriteMessage(data); //原消息返回
  362. //触发Wago对应数据更新
  363. UpdateDataCausedByWago(position, status);
  364. return;
  365. }
  366. else if (command == 0x06)//写AO
  367. {
  368. short startAddress = byteTransform.TransInt16(data, 8);
  369. if(startAddress > 0x02FF || startAddress < WRITE_AO_STARTADDRESS)
  370. {
  371. short flag = byteTransform.TransInt16(data, 0);
  372. byte channel = data[6];
  373. OnWriteMessage(CreateError(flag, channel, command, 0x02)); //地址错误
  374. return;
  375. }
  376. int position = startAddress - WRITE_AO_STARTADDRESS;
  377. short value = byteTransform.TransInt16(data, 10);
  378. lock (_writeAOLocker)
  379. {
  380. AOShorts[position] = value;
  381. }
  382. OnWriteMessage(data); //原消息返回
  383. return;
  384. }
  385. else
  386. {
  387. short flag = byteTransform.TransInt16(data, 0);
  388. byte channel = data[6];
  389. OnWriteMessage(CreateError(flag, channel, command, 0x01)); //指令错误
  390. return;
  391. }
  392. }
  393. /// <summary>
  394. /// 回复读数字量
  395. /// </summary>
  396. /// <param name="flag"></param>
  397. /// <param name="channel"></param>
  398. /// <param name="command"></param>
  399. /// <param name="byteCount"></param>
  400. /// <param name="values"></param>
  401. /// <returns></returns>
  402. private byte[] CreateReadDigitalResponse(short flag, byte channel, byte command, byte byteCount, byte[] values)
  403. {
  404. byte[] bytes = new byte[7 + 2 + values.Length]; //回复字节长度,前面7个字节固定长度 + functionCode一个字节 + byteCount一个字节+values.length个字节
  405. Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
  406. bytes[2] = 0x00;
  407. bytes[3] = 0x00;
  408. short dataLength = (short)(3 + values.Length);
  409. Array.Copy(byteTransform.GetBytes(dataLength), 0, bytes, 4, 2);
  410. bytes[6] = channel;
  411. bytes[7] = command;
  412. bytes[8] = byteCount;
  413. Array.Copy(values, 0, bytes, 9, values.Length);
  414. return bytes;
  415. }
  416. /// <summary>
  417. /// 回复读模拟量
  418. /// </summary>
  419. /// <param name="flag"></param>
  420. /// <param name="channel"></param>
  421. /// <param name="command"></param>
  422. /// <param name="registerCount"></param>
  423. /// <param name="values"></param>
  424. /// <returns></returns>
  425. private byte[] CreateReadAnalogyResponse(short flag, byte channel, byte command, byte registerCount, byte[] values)
  426. {
  427. byte[] bytes = new byte[7 + 2 + 2 * registerCount]; //回复字节长度,前面7个字节固定长度 + functionCode一个字节 + byteCount一个字节+registerCount*2个字节(一个寄存器占两个字节)
  428. Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
  429. bytes[2] = 0x00;
  430. bytes[3] = 0x00;
  431. short dataLength = (short)(3 + 2 * registerCount);
  432. Array.Copy(byteTransform.GetBytes(dataLength), 0, bytes, 4, 2);
  433. bytes[6] = channel;
  434. bytes[7] = command;
  435. bytes[8] = (byte)(2 * registerCount);
  436. Array.Copy(values, 0, bytes, 9, values.Length);
  437. return bytes;
  438. }
  439. /// <summary>
  440. /// 错误回复
  441. /// </summary>
  442. /// <param name="flag"></param>
  443. /// <param name="channel"></param>
  444. /// <param name="command"></param>
  445. /// <param name="error"></param>
  446. /// <returns></returns>
  447. private byte[] CreateError(short flag, byte channel, byte command, byte error)
  448. {
  449. byte[] bytes = new byte[9];
  450. Array.Copy(byteTransform.GetBytes(flag), 0, bytes, 0, 2);
  451. bytes[2] = 0x00;
  452. bytes[3] = 0x00;
  453. bytes[4] = 0x00;
  454. bytes[5] = 0x03;
  455. bytes[6] = channel;
  456. bytes[7] = (byte)(command | 0x80);
  457. bytes[8] = error;
  458. return bytes;
  459. }
  460. /// <summary>
  461. /// 定时器
  462. /// </summary>
  463. /// <returns></returns>
  464. private bool OnTimer()
  465. {
  466. LeakTestSimulator();
  467. return true;
  468. }
  469. #region 模拟方法
  470. /// <summary>
  471. /// Loader LeakTest模拟
  472. /// </summary>
  473. private void LeakTestSimulator()
  474. {
  475. if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 1 && _festoDataBuffer["FlowTestClamp"] && AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] == 0)
  476. {
  477. AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] = 15000;
  478. }
  479. else if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 0 && !_festoDataBuffer["FlowTestClamp"])
  480. {
  481. AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] = 0;
  482. }
  483. if (DOBytes[DONameIndexDic["c_VACUUM_TEST"]] == 1 && _festoDataBuffer["FlowTestClamp"] && AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] > 3500)
  484. {
  485. AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] -= 40;
  486. if (AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] < 14000)
  487. {
  488. AIShorts[AINameIndexDic["r_LOADER_GasFlowSensor_FLOW"]] -= 140;
  489. }
  490. }
  491. }
  492. #endregion
  493. }
  494. }