IoTriStateLift2.cs 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415
  1. using Aitex.Core.RT.Device;
  2. using Aitex.Core.RT.Event;
  3. using Aitex.Core.RT.IOCore;
  4. using System.Xml;
  5. using System.Diagnostics;
  6. using Aitex.Core.Common.DeviceData;
  7. using Aitex.Core.RT.DataCenter;
  8. using Aitex.Core.RT.SCCore;
  9. using Aitex.Core.RT.OperationCenter;
  10. using VirgoCommon;
  11. namespace VirgoRT.Devices
  12. {
  13. public class IoTriStateLift2 : BaseDevice, IDevice
  14. {
  15. private readonly DIAccessor _diOrigin;//是否到达原点
  16. private readonly DIAccessor _diOrigin2;
  17. private readonly DIAccessor _diP1;//是否到达位置1
  18. private readonly DIAccessor _diP2;//是否到达位置2
  19. private readonly DIAccessor _diP3;//是否到达位置3
  20. private readonly DIAccessor _diCOMMAlarm;//去位置1,2,3超时
  21. private readonly DIAccessor _diBatteryLowAlarm;//去原点超时
  22. // private readonly DIAccessor _diServoAlarm; // servo alarm
  23. // private readonly DIAccessor _diCCWLimitSensorAlarm; //
  24. //private readonly DIAccessor _diOverSoftwareLimitAlarm; //
  25. private readonly DOAccessor _doReset;
  26. private readonly DOAccessor _doOrigin;//去原点
  27. private readonly DOAccessor _doP1;//去位置1
  28. private readonly DOAccessor _doP2;//去位置2
  29. private readonly DOAccessor _doP3;//去位置3
  30. private readonly DOAccessor _doStop;
  31. private readonly DOAccessor _doUp;
  32. private readonly DOAccessor _doDown;
  33. private readonly AIAccessor _currentValue;
  34. private readonly AOAccessor _aoSetP1;
  35. private readonly AOAccessor _aoSetP2;
  36. private readonly AOAccessor _aoSetP3;
  37. private readonly AOAccessor _aoServoEnable;
  38. private readonly AOAccessor _aoServoWorkMode;
  39. private readonly AOAccessor _aoOriginSpeed;
  40. private readonly AOAccessor _aoAutoSpeed;
  41. private readonly AOAccessor _aoManualSpeed;
  42. private readonly AOAccessor _aoAccDecSpeedTime;
  43. private readonly AOAccessor _aoPosDiff;
  44. private readonly AOAccessor _aoDecTime;
  45. private readonly AOAccessor _aoSoftUpLimit;
  46. private readonly AOAccessor _aoSoftDownLimit;
  47. private readonly AOAccessor _aoCorrectionValue;
  48. private Stopwatch sw = new Stopwatch();
  49. private Stopwatch _manualStopTimer = new Stopwatch();
  50. private readonly int _stopButtonAutoResetTime = 1000;
  51. private Position _currentTarget = Position.Invalid;
  52. long _timeout = 10000;
  53. private bool _bAlarmReported = false;
  54. private AITTriStateLiftPinData DeviceData
  55. {
  56. get
  57. {
  58. AITTriStateLiftPinData deviceData = new AITTriStateLiftPinData
  59. {
  60. Module = Module,
  61. DeviceName = Name,
  62. DeviceSchematicId = DeviceID,
  63. DisplayName = Display,
  64. };
  65. return deviceData;
  66. }
  67. }
  68. public MovementPosition PinPosition
  69. {
  70. get
  71. {
  72. if (_diP1.Value && _diP2.Value == false && _diP3.Value == false)
  73. return MovementPosition.Up;
  74. else if (_diP1.Value == false && _diP2.Value && _diP3.Value == false)
  75. return MovementPosition.Middle;
  76. else if (_diP1.Value == false && _diP2.Value == false && _diP3.Value)
  77. return MovementPosition.Down;
  78. else if (_diOrigin.Value && _diOrigin2.Value)
  79. return MovementPosition.Origin;
  80. return MovementPosition.Unknown;
  81. }
  82. }
  83. public int PinPositionint
  84. {
  85. get
  86. {
  87. if (PinPosition == MovementPosition.Up)
  88. return 3;
  89. else if (PinPosition == MovementPosition.Middle)
  90. return 2;
  91. else if (PinPosition == MovementPosition.Down)
  92. return 1;
  93. else if (PinPosition == MovementPosition.Origin)
  94. return 0;
  95. return -1;
  96. }
  97. }
  98. public IoTriStateLift2(string module, XmlElement node, string ioModule = "")
  99. {
  100. base.Module = module;
  101. base.Name = node.GetAttribute("id");
  102. base.Display = node.GetAttribute("display");
  103. base.DeviceID = node.GetAttribute("schematicId");
  104. _diOrigin = ParseDiNode("diOrigin", node, ioModule);
  105. _diOrigin2 = ParseDiNode("diOrigin2", node, ioModule);
  106. _diP1 = ParseDiNode("diP1", node, ioModule);
  107. _diP2 = ParseDiNode("diP2", node, ioModule);
  108. _diP3 = ParseDiNode("diP3", node, ioModule);
  109. _diCOMMAlarm = ParseDiNode("diCOMMAlarm", node, ioModule);
  110. _diBatteryLowAlarm = ParseDiNode("diBatteryLowAlarm", node, ioModule);
  111. _doReset = ParseDoNode("doReset", node, ioModule);
  112. _doOrigin = ParseDoNode("doOrigin", node, ioModule);
  113. _doP1 = ParseDoNode("doP1", node, ioModule);
  114. _doP2 = ParseDoNode("doP2", node, ioModule);
  115. _doP3 = ParseDoNode("doP3", node, ioModule);
  116. _doStop = ParseDoNode("doStop", node, ioModule);
  117. _doUp = ParseDoNode("doUp", node, ioModule);
  118. _doDown = ParseDoNode("doDown", node, ioModule);
  119. _currentValue = ParseAiNode("aiCurrentValue", node, ioModule);
  120. _aoSetP1 = ParseAoNode("aoSetP1", node, ioModule);
  121. _aoSetP2 = ParseAoNode("aoSetP2", node, ioModule);
  122. _aoSetP3 = ParseAoNode("aoSetP3", node, ioModule);
  123. _aoServoEnable = ParseAoNode("aoServoEnable", node, ioModule);
  124. _aoServoWorkMode = ParseAoNode("aoServoWorkMode", node, ioModule);
  125. _aoOriginSpeed = ParseAoNode("aoOriginSpeed", node, ioModule);
  126. _aoAutoSpeed = ParseAoNode("aoAutoSpeed", node, ioModule);
  127. _aoManualSpeed = ParseAoNode("aoManualSpeed", node, ioModule);
  128. _aoSoftUpLimit = ParseAoNode("aoSoftUpLimit", node, ioModule);
  129. _aoSoftDownLimit = ParseAoNode("aoSoftDownLimit", node, ioModule);
  130. _aoAccDecSpeedTime = ParseAoNode("aoAccDecSpeedTime", node, ioModule);
  131. _aoPosDiff = ParseAoNode("aoPosDiff", node, ioModule);
  132. }
  133. private void updatePinCfg()
  134. {
  135. // AO-27, Lift Servo Enable: 0=Lift Pin ,1=Lift Servo
  136. _SetRealFloat(_aoServoEnable, 1);
  137. void _updateItem(string data, AOAccessor ao)
  138. {
  139. var value = (float)SC.GetValue<double>($"{Module}.{Name}.{data}");
  140. _SetRealFloat(ao, value);
  141. }
  142. _updateItem("ServoWorkMode", _aoOriginSpeed);
  143. _updateItem("OriginSpeed", _aoOriginSpeed);
  144. _updateItem("AutoSpeed", _aoAutoSpeed);
  145. _updateItem("ManualSpeed", _aoManualSpeed);
  146. _updateItem("SoftUpLimit", _aoSoftUpLimit);
  147. _updateItem("SoftDownLimit", _aoSoftDownLimit);
  148. _updateItem("AccDecSpeedTime", _aoAccDecSpeedTime);
  149. _updateItem("Position1", _aoSetP1);
  150. _updateItem("Position2", _aoSetP2);
  151. _updateItem("Position3", _aoSetP3);
  152. _updateItem("PosDiff", _aoPosDiff);
  153. }
  154. public bool GoPosition(Position position)
  155. {
  156. if (_diCOMMAlarm.Value)
  157. return false;
  158. _currentTarget = position;
  159. sw.Restart();
  160. switch (position)
  161. {
  162. case Position.position1:
  163. _doP1.Value = true;
  164. _doP2.Value = false;
  165. _doP3.Value = false;
  166. break;
  167. case Position.position2:
  168. _doP1.Value = false;
  169. _doP2.Value = true;
  170. _doP3.Value = false;
  171. break;
  172. case Position.position3:
  173. _doP1.Value = false;
  174. _doP2.Value = false;
  175. _doP3.Value = true;
  176. break;
  177. case Position.origin:
  178. {
  179. if (_diOrigin.Value && _diOrigin2.Value)
  180. {
  181. EV.PostInfoLog(Module, $"Lift Pin already on original position.");
  182. sw.Stop();
  183. return true;
  184. }
  185. _doOrigin.Value = true;
  186. }
  187. break;
  188. }
  189. EV.PostInfoLog(Module, $"Lift Pin goto {_currentTarget}");
  190. return true;
  191. }
  192. public bool ManulStop()
  193. {
  194. if (_diCOMMAlarm.Value)
  195. return false;
  196. _doUp.Value = false;
  197. _doDown.Value = false;
  198. _doStop.Value = true;
  199. _manualStopTimer.Restart();
  200. return true;
  201. }
  202. public bool ManulUp()
  203. {
  204. if (_diCOMMAlarm.Value)
  205. return false;
  206. _doDown.Value = false;
  207. _doStop.Value = false;
  208. _doUp.Value = !_doUp.Value;
  209. return true;
  210. }
  211. public bool ManulDown()
  212. {
  213. if (_diCOMMAlarm.Value)
  214. return false;
  215. _doUp.Value = false;
  216. _doStop.Value = false;
  217. _doDown.Value = !_doDown.Value;
  218. return true;
  219. }
  220. public float CurrentValue
  221. {
  222. get
  223. {
  224. if (_currentValue == null)
  225. {
  226. return 0;
  227. }
  228. return _GetRealFloat(_currentValue); ;
  229. }
  230. }
  231. public bool Initialize()
  232. {
  233. DATA.Subscribe($"{Module}.{Name}.DeviceData", () => DeviceData);
  234. DATA.Subscribe($"{Module}.{Name}.PinPosition", () => PinPositionint);
  235. DATA.Subscribe($"{Module}.{Name}.CurrentValue", () => CurrentValue);
  236. DATA.Subscribe($"{Module}.{Name}.ManualStopState", () => _doStop.Value);
  237. DATA.Subscribe($"{Module}.{Name}.ManualUpState", () => _doUp.Value);
  238. DATA.Subscribe($"{Module}.{Name}.ManualDownState", () => _doDown.Value);
  239. OP.Subscribe($"{Module}.{Name}.SetState", (out string reason, int time, object[] param) => {
  240. reason = string.Empty;
  241. Position pos = Position.Invalid;
  242. if ((string)param[0] == "Up")
  243. pos = Position.position1;
  244. else if ((string)param[0] == "Down")
  245. pos = Position.position3;
  246. else if ((string)param[0] == "Middle")
  247. pos = Position.position2;
  248. else
  249. {
  250. reason = "Invalid moving position";
  251. return false;
  252. }
  253. GoPosition(pos);
  254. return true;
  255. });
  256. OP.Subscribe($"{Module}.{Name}.Stop", (out string reason, int time, object[] param) => {
  257. reason = string.Empty;
  258. ManulStop();
  259. return true;
  260. });
  261. OP.Subscribe($"{Module}.{Name}.Up", (out string reason, int time, object[] param) => {
  262. reason = string.Empty;
  263. ManulUp();
  264. return true;
  265. });
  266. OP.Subscribe($"{Module}.{Name}.Down", (out string reason, int time, object[] param) => {
  267. reason = string.Empty;
  268. ManulDown();
  269. return true;
  270. });
  271. OP.Subscribe($"{Module}.{Name}.Home", (out string reason, int time, object[] param) => {
  272. reason = string.Empty;
  273. GoPosition(Position.origin);
  274. return true;
  275. });
  276. OP.Subscribe($"{Module}.{Name}.UpdateConfig", (out string reason, int time, object[] param) => {
  277. reason = string.Empty;
  278. return true;
  279. });
  280. updatePinCfg();
  281. return true;
  282. }
  283. public void Terminate()
  284. {
  285. }
  286. public void Monitor()
  287. {
  288. if (_manualStopTimer.ElapsedMilliseconds > _stopButtonAutoResetTime)
  289. {
  290. _doStop.Value = false;
  291. _manualStopTimer.Stop();
  292. }
  293. if (_diBatteryLowAlarm.Value)
  294. {
  295. NoDuplicatedAlarm($"Lift Pin DI-{_diBatteryLowAlarm.Index} alarm");
  296. }
  297. if (_diCOMMAlarm.Value)
  298. {
  299. NoDuplicatedAlarm($"Lift Pin DI-{_diCOMMAlarm.Index} alarm");
  300. }
  301. if (_currentTarget == Position.Invalid)
  302. return;
  303. if ((_currentTarget == Position.position1 && _diP1.Value) ||
  304. (_currentTarget == Position.position2 && _diP2.Value) ||
  305. (_currentTarget == Position.position3 && _diP3.Value) ||
  306. (_currentTarget == Position.origin && (_diOrigin.Value && _diOrigin2.Value)))
  307. {
  308. EV.PostInfoLog(Module, $"Lift Pin arrive {_currentTarget}");
  309. Reset();
  310. return;
  311. }
  312. if (sw.ElapsedMilliseconds > _timeout)
  313. {
  314. NoDuplicatedAlarm($"Lift Pin timeout, go {_currentTarget} failed");
  315. }
  316. }
  317. public void Reset()
  318. {
  319. _currentTarget = Position.Invalid;
  320. sw.Reset();
  321. _doP1.Value = false;
  322. _doP2.Value = false;
  323. _doP3.Value = false;
  324. _doOrigin.Value = false;
  325. _doUp.Value = false;
  326. _doDown.Value = false;
  327. _doStop.Value = false;
  328. _bAlarmReported = false;
  329. EV.PostInfoLog(Module, $"Lift Pin reset all do to off.");
  330. }
  331. private void NoDuplicatedAlarm(string log)
  332. {
  333. if (_bAlarmReported == false)
  334. {
  335. EV.PostAlarmLog(Module, log);
  336. _bAlarmReported = true;
  337. }
  338. }
  339. }
  340. }