IoFurnaceMotor.cs 39 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998
  1. using Aitex.Core.RT.DataCenter;
  2. using Aitex.Core.RT.Device;
  3. using Aitex.Core.RT.Event;
  4. using Aitex.Core.RT.IOCore;
  5. using Aitex.Core.RT.Log;
  6. using Aitex.Core.RT.OperationCenter;
  7. using Aitex.Core.RT.SCCore;
  8. using Aitex.Core.Util;
  9. using DocumentFormat.OpenXml.Spreadsheet;
  10. using FurnaceRT.Equipments.Boats;
  11. using FurnaceRT.Equipments.Systems;
  12. using MECF.Framework.Common.Equipment;
  13. using System;
  14. using System.Collections;
  15. using System.Collections.Generic;
  16. using System.Linq;
  17. using System.Text;
  18. using System.Threading.Tasks;
  19. using System.Xml;
  20. namespace FurnaceRT.Devices
  21. {
  22. public class IoFurnaceMotor : BaseDevice, IDevice
  23. {
  24. #region fields
  25. protected enum State
  26. {
  27. Idle,
  28. MoveToPosition,
  29. Rotating,
  30. Homing,
  31. Jogging,
  32. Pitching,
  33. Stopping,
  34. ServoOff,
  35. ServoOn,
  36. ServoReset,
  37. }
  38. public enum Direction
  39. {
  40. Unknown,
  41. CW,
  42. CCW,
  43. }
  44. protected State _state = State.Idle;
  45. protected DIAccessor _diHoming;
  46. protected DIAccessor _diInitializing;
  47. protected DIAccessor _diHomeDone;
  48. protected DIAccessor _diInitDone;
  49. protected DIAccessor _diMoving;
  50. protected DIAccessor _diMotorWarning;
  51. protected DIAccessor _diMotorAlarm;
  52. protected DIAccessor _diAlarm;
  53. protected DIAccessor _diServoOn;
  54. protected DIAccessor _diDirection;
  55. protected DIAccessor _diM0;
  56. protected DIAccessor _diM1;
  57. protected DIAccessor _diM2;
  58. protected DIAccessor _diM3;
  59. protected DIAccessor _diM4;
  60. protected DIAccessor _diM5;
  61. protected DIAccessor _diHomePosition;
  62. protected DIAccessor _diNegativeLimit;
  63. protected DIAccessor _diPositiveLimit;
  64. protected DIAccessor _diPosition1;
  65. protected DIAccessor _diPosition2;
  66. protected DIAccessor _diPosition3;
  67. protected DOAccessor _doStop;
  68. protected DOAccessor _doHome;
  69. protected DOAccessor _doInit;
  70. protected DOAccessor _doMove;
  71. protected DOAccessor _doReset;
  72. protected DOAccessor _doServoOn;
  73. protected DOAccessor _doCW;
  74. protected DOAccessor _doCCW;
  75. protected DOAccessor _doM0;
  76. protected DOAccessor _doM1;
  77. protected DOAccessor _doM2;
  78. protected DOAccessor _doM3;
  79. protected DOAccessor _doM4;
  80. protected DOAccessor _doM5;
  81. protected AIAccessor _aiRealPosition;
  82. protected AIAccessor _aiRealSpeed;
  83. protected AIAccessor _aiBoatElevatorErrorCode;
  84. protected AIAccessor _aiTargetSpeed;
  85. protected AIAccessor _aiDriverErrorCode;
  86. protected AIAccessor _aiMotionErrorCode;
  87. protected AIAccessor _aiTargetPosFb;
  88. protected AOAccessor _aoTargetPosition;
  89. protected AOAccessor _aoTargetSpeed;
  90. protected AOAccessor _aoAcc;
  91. protected AOAccessor _aoDec;
  92. protected SCConfigItem _scServoMoveSpeed;
  93. protected SCConfigItem _scServoAcc;
  94. protected SCConfigItem _scServoDec;
  95. protected SCConfigItem _scServoPosition1;
  96. protected SCConfigItem _scServoPosition2;
  97. protected SCConfigItem _scServoPosition3;
  98. protected SCConfigItem _scServoPosition4;
  99. protected SCConfigItem _scServoPosition5;
  100. protected SCConfigItem _scServoPosition6;
  101. protected SCConfigItem _scServoPosition7;
  102. protected SCConfigItem _scServoPosition8;
  103. protected SCConfigItem _scServoPosition9;
  104. protected SCConfigItem _scServoPosition10;
  105. protected SCConfigItem _scMoveTimeout;
  106. protected DeviceTimer _timer = new DeviceTimer();
  107. private const float PositionTolerance = 0.1f;
  108. private int setTime = 100; //200ms set DO
  109. private int resetTime = 2000; //2000 reset DO
  110. private int moveTimeout => (int)(_scMoveTimeout.IntValue * 1000);
  111. private PeriodicJob _thread;
  112. private R_TRIG _stateTrig = new R_TRIG();
  113. private R_TRIG _negativeLimitTrig = new R_TRIG();
  114. private R_TRIG _positiveLimitTrig = new R_TRIG();
  115. private R_TRIG _warningTrig = new R_TRIG();
  116. private R_TRIG _alarmTrig = new R_TRIG();
  117. private string _lastTarget = "";
  118. private bool IsRAxis = false;
  119. public bool ServoOnOffSet
  120. {
  121. set
  122. {
  123. if (_doServoOn == null)
  124. return;
  125. _doServoOn.Value = value;
  126. }
  127. }
  128. public float ServoMoveSpeedSet
  129. {
  130. set
  131. {
  132. if (_aoTargetSpeed == null)
  133. return;
  134. _aoTargetSpeed.FloatValue = value;
  135. }
  136. }
  137. public float ServoAccSet
  138. {
  139. set
  140. {
  141. if (_aoAcc == null)
  142. return;
  143. _aoAcc.FloatValue = value;
  144. }
  145. }
  146. public float ServoDecSet
  147. {
  148. set
  149. {
  150. if (_aoDec == null)
  151. return;
  152. _aoDec.FloatValue = value;
  153. }
  154. }
  155. public float ServoMovePositionSet
  156. {
  157. set
  158. {
  159. if (_aoTargetPosition == null)
  160. return;
  161. _aoTargetPosition.FloatValue = value;
  162. }
  163. get
  164. {
  165. return _aoTargetPosition.FloatValue;
  166. }
  167. }
  168. public Direction MotorDirection
  169. {
  170. get
  171. {
  172. if (_diDirection != null)
  173. return _diDirection.Value ? Direction.CCW : Direction.CW;
  174. if (_doCW != null && _doCCW != null)
  175. {
  176. if (_doCW.Value)
  177. return Direction.CW;
  178. if (_doCCW.Value)
  179. return Direction.CCW;
  180. }
  181. return Direction.Unknown;
  182. }
  183. }
  184. public int TargetPositionFb => (int)(_aiTargetPosFb.FloatValue + 0.00001);
  185. public string ErrorCode => $"{(_aiDriverErrorCode != null ? ((int)(_aiDriverErrorCode.FloatValue + 0.00001)).ToString("X") : "")}/{(_aiMotionErrorCode != null ? ((int)(_aiMotionErrorCode.FloatValue + 0.00001)).ToString("X") : "")}";
  186. private bool _isFloatAioType = false;
  187. #endregion
  188. #region properties
  189. public float CurrentPosition => _aiRealPosition.FloatValue;
  190. public float CurrentSpeed => _aiRealSpeed.FloatValue;
  191. public Dictionary<string, string> IoMappingDic { set; get; }
  192. public bool IsError
  193. {
  194. get
  195. {
  196. if (_diMotorAlarm != null && _diMotorAlarm.Value)
  197. return true;
  198. if (_diAlarm != null && _diAlarm.Value)
  199. return true;
  200. return false;
  201. }
  202. }
  203. public bool IsHomeDone => _diHomeDone == null ? false : _diHomeDone.Value;
  204. public bool IsInitDone => _diInitDone == null ? false : _diInitDone.Value;
  205. public bool IsMoving => _diMoving == null ? false : _diMoving.Value;
  206. public bool IsPause { get; set; }
  207. public bool IsReady
  208. {
  209. get
  210. {
  211. if (_diMotorWarning != null && _diMotorWarning.Value)
  212. return false;
  213. if (_diMotorAlarm != null && _diMotorAlarm.Value)
  214. return false;
  215. if (_diAlarm != null && _diAlarm.Value)
  216. return false;
  217. if (_diServoOn != null && !_diServoOn.Value)
  218. return false;
  219. if (_diInitializing != null && _diInitializing.Value)
  220. return false;
  221. if (_diHoming != null && _diHoming.Value)
  222. return false;
  223. if (_diMoving != null && _diMoving.Value)
  224. return false;
  225. return _state == State.Idle;
  226. }
  227. }
  228. public bool IsServoOn
  229. {
  230. get
  231. {
  232. if (_diServoOn == null)
  233. return true;
  234. return _diServoOn.Value;
  235. }
  236. }
  237. #endregion
  238. public IoFurnaceMotor(string module, XmlElement node, string ioModule = "")
  239. {
  240. base.Module = string.IsNullOrEmpty(node.GetAttribute("module")) ? module : node.GetAttribute("module");
  241. base.Name = node.GetAttribute("id");
  242. base.Display = node.GetAttribute("display");
  243. base.DeviceID = node.GetAttribute("schematicId");
  244. _isFloatAioType = !string.IsNullOrEmpty(node.GetAttribute("aioType")) && (node.GetAttribute("aioType") == "float");
  245. var scRootPath = string.IsNullOrEmpty(node.GetAttribute("scRootPath")) ? Module : node.GetAttribute("scRootPath");
  246. _diHoming = ParseDiNode("diHoming", node, ioModule);
  247. _diInitializing = ParseDiNode("diInitializing", node, ioModule);
  248. _diHomeDone = ParseDiNode("diHomeDone", node, ioModule);
  249. _diInitDone = ParseDiNode("diInitDone", node, ioModule);
  250. _diMoving = ParseDiNode("diMoving", node, ioModule);
  251. _diMotorWarning = ParseDiNode("diMotorWarning", node, ioModule);
  252. _diMotorAlarm = ParseDiNode("diMotorAlarm", node, ioModule);
  253. _diAlarm = ParseDiNode("diAlarm", node, ioModule);
  254. _diServoOn = ParseDiNode("diServoOn", node, ioModule);
  255. _diDirection = ParseDiNode("diDirection", node, ioModule);
  256. _diM0 = ParseDiNode("diM0", node, ioModule);
  257. _diM1 = ParseDiNode("diM1", node, ioModule);
  258. _diM2 = ParseDiNode("diM2", node, ioModule);
  259. _diM3 = ParseDiNode("diM3", node, ioModule);
  260. _diM4 = ParseDiNode("diM4", node, ioModule);
  261. _diM5 = ParseDiNode("diM5", node, ioModule);
  262. _diHomePosition = ParseDiNode("diHomePosition", node, ioModule);
  263. _diNegativeLimit = ParseDiNode("diNegativeLimit", node, ioModule);
  264. _diPositiveLimit = ParseDiNode("diPositiveLimit", node, ioModule);
  265. _diPosition1 = ParseDiNode("diPosition1", node, ioModule);
  266. _diPosition2 = ParseDiNode("diPosition2", node, ioModule);
  267. _diPosition3 = ParseDiNode("diPosition3", node, ioModule);
  268. _doStop = ParseDoNode("doStop", node, ioModule);
  269. _doHome = ParseDoNode("doHome", node, ioModule);
  270. _doInit = ParseDoNode("doInit", node, ioModule);
  271. _doMove = ParseDoNode("doMove", node, ioModule);
  272. _doReset = ParseDoNode("doReset", node, ioModule);
  273. _doServoOn = ParseDoNode("doServoOn", node, ioModule);
  274. _doCW = ParseDoNode("doCW", node, ioModule);
  275. _doCCW = ParseDoNode("doCCW", node, ioModule);
  276. _doM0 = ParseDoNode("doM0", node, ioModule);
  277. _doM1 = ParseDoNode("doM1", node, ioModule);
  278. _doM2 = ParseDoNode("doM2", node, ioModule);
  279. _doM3 = ParseDoNode("doM3", node, ioModule);
  280. _doM4 = ParseDoNode("doM4", node, ioModule);
  281. _doM5 = ParseDoNode("doM5", node, ioModule);
  282. _aiRealPosition = ParseAiNode("aiRealPosition", node, ioModule);
  283. _aiRealSpeed = ParseAiNode("aiRealSpeed", node, ioModule);
  284. _aiBoatElevatorErrorCode = ParseAiNode("aiBoatElevatorErrorCode", node, ioModule);
  285. _aiTargetSpeed = ParseAiNode("aiTargetSpeed", node, ioModule);
  286. _aiDriverErrorCode = ParseAiNode("aiDriverErrorCode", node, ioModule);
  287. _aiMotionErrorCode = ParseAiNode("aiMotionErrorCode", node, ioModule);
  288. _aiTargetPosFb = ParseAiNode("aiTargetPosFb", node, ioModule);
  289. _aoTargetPosition = ParseAoNode("aoTargetPosition", node, ioModule);
  290. _aoTargetSpeed = ParseAoNode("aoTargetSpeed", node, ioModule);
  291. _aoAcc = ParseAoNode("aoAcc", node, ioModule);
  292. _aoDec = ParseAoNode("aoDec", node, ioModule);
  293. _scServoMoveSpeed = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.MoveSpeed");
  294. _scServoAcc = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.Acc");
  295. _scServoDec = ParseScNode("scSpeed1", node, ioModule, $"{scRootPath}.Dec");
  296. _scServoPosition1 = ParseScNode("scServoPosition1", node, ioModule, $"{scRootPath}.Position1");
  297. _scServoPosition2 = ParseScNode("scServoPosition2", node, ioModule, $"{scRootPath}.Position2");
  298. _scServoPosition3 = ParseScNode("scServoPosition3", node, ioModule, $"{scRootPath}.Position3");
  299. _scServoPosition4 = ParseScNode("scServoPosition4", node, ioModule, $"{scRootPath}.Position4");
  300. _scServoPosition5 = ParseScNode("scServoPosition5", node, ioModule, $"{scRootPath}.Position5");
  301. _scServoPosition6 = ParseScNode("scServoPosition6", node, ioModule, $"{scRootPath}.Position6");
  302. _scServoPosition7 = ParseScNode("scServoPosition7", node, ioModule, $"{scRootPath}.Position7");
  303. _scServoPosition8 = ParseScNode("scServoPosition8", node, ioModule, $"{scRootPath}.Position8");
  304. _scServoPosition9 = ParseScNode("scServoPosition9", node, ioModule, $"{scRootPath}.Position9");
  305. _scServoPosition10 = ParseScNode("scServoPosition10", node, ioModule, $"{scRootPath}.Position10");
  306. _scMoveTimeout = ParseScNode("scMoveTimeout", node, ioModule, $"{scRootPath}.MotionTimeout");
  307. IsRAxis = Name == "BoatRotationServo";
  308. _thread = new PeriodicJob(50, OnTimer, Name);
  309. _thread.Start();
  310. }
  311. public virtual bool Initialize()
  312. {
  313. DATA.Subscribe($"{Module}.{Name}.CurrentPosition", () => _aiRealPosition != null ? (_isFloatAioType ? _aiRealPosition.FloatValue : _aiRealPosition.Value) : 0);
  314. DATA.Subscribe($"{Module}.{Name}.CurrentSpeed", () => GetAxisSpeed());
  315. if (_aiTargetPosFb != null)
  316. DATA.Subscribe($"{Module}.{Name}.TargetPositionFb", () => _aiTargetPosFb != null ? (_isFloatAioType ? _aiTargetPosFb.FloatValue : _aiTargetPosFb.Value) : 0);
  317. DATA.Subscribe($"{Module}.{Name}.TargetPosition", () => _aoTargetPosition != null ? (_isFloatAioType ? _aoTargetPosition.FloatValue : _aoTargetPosition.Value) : 0);
  318. DATA.Subscribe($"{Module}.{Name}.IsReady", () => IsReady);
  319. DATA.Subscribe($"{Module}.{Name}.IsWarning", () => _diMotorWarning != null ? _diMotorWarning.Value : false);
  320. DATA.Subscribe($"{Module}.{Name}.IsAlarm", () => IsError);
  321. DATA.Subscribe($"{Module}.{Name}.IsServoOn", () => _diServoOn != null ? _diServoOn.Value : false);
  322. DATA.Subscribe($"{Module}.{Name}.ErrorCode", () => ErrorCode);
  323. DATA.Subscribe($"{Module}.{Name}.Status", () => _state.ToString());
  324. DATA.Subscribe($"{Module}.{Name}.Direction", () => MotorDirection.ToString());
  325. DATA.Subscribe($"{Module}.{Name}.IsMoving", () => _diMoving != null ? _diMoving.Value : false);
  326. DATA.Subscribe($"{Module}.{Name}.IsInitDone", () => _diInitDone != null ? _diInitDone.Value : false);
  327. DATA.Subscribe($"{Module}.{Name}.IsHomeDone", () => _diHomeDone != null ? _diHomeDone.Value : false);
  328. DATA.Subscribe($"{Module}.{Name}.IsHoming", () => _diHoming != null ? _diHoming.Value : false);
  329. DATA.Subscribe($"{Module}.{Name}.IsInitializing", () => _diInitializing != null ? _diInitializing.Value : false);
  330. DATA.Subscribe($"{Module}.{Name}.AtPosition1", () => _diPosition1 != null ? _diPosition1.Value : false);
  331. DATA.Subscribe($"{Module}.{Name}.AtPosition2", () => _diPosition2 != null ? _diPosition2.Value : false);
  332. DATA.Subscribe($"{Module}.{Name}.AtPosition3", () => _diPosition3 != null ? _diPosition3.Value : false);
  333. DATA.Subscribe($"{Module}.{Name}.AtHomePosition", () => _diHomePosition != null ? _diHomePosition.Value : false);
  334. OP.Subscribe($"{Module}.{Name}.SetServoPara", (string cmd, object[] param) =>
  335. {
  336. float.TryParse(param[1].ToString(), out float value);
  337. SetServoPara(param[0].ToString(), value);
  338. return true;
  339. });
  340. OP.Subscribe($"{Module}.{Name}.ServoMoveTo", (string cmd, object[] param) =>
  341. {
  342. //if (!IsServoOn)
  343. //{
  344. // EV.PostWarningLog($"{Module}", $"{Name} servo not on");
  345. // return false;
  346. //}
  347. if (_state != State.Idle)
  348. {
  349. EV.PostWarningLog($"{Module}", $"{Name} busy, wait");
  350. return false;
  351. }
  352. float setSpeed = 0.0f;
  353. if (param.Length >= 2)
  354. {
  355. float.TryParse(param[1].ToString(), out setSpeed);
  356. }
  357. SetServoMoveTo(param[0].ToString(), out _, setSpeed);
  358. return true;
  359. });
  360. OP.Subscribe($"{Module}.{Name}.ServoHome", (string cmd, object[] param) =>
  361. {
  362. //if (!IsServoOn)
  363. //{
  364. // EV.PostWarningLog($"{Module}", $"{Name} servo not on");
  365. // return false;
  366. //}
  367. if (_state != State.Idle)
  368. {
  369. EV.PostWarningLog($"{Module}", $"{Name} busy, wait");
  370. return false;
  371. }
  372. SetServoHome();
  373. return true;
  374. });
  375. OP.Subscribe($"{Module}.{Name}.ServoOnOff", (string cmd, object[] param) =>
  376. {
  377. bool.TryParse(param[0].ToString(), out bool isOn);
  378. SetServoOnOff(isOn);
  379. return true;
  380. });
  381. OP.Subscribe($"{Module}.{Name}.ServoResetAlarm", (string cmd, object[] param) =>
  382. {
  383. ServoReset(out _);
  384. return true;
  385. });
  386. OP.Subscribe($"{Module}.{Name}.ServoStop", (string cmd, object[] param) =>
  387. {
  388. ServoStop();
  389. return true;
  390. });
  391. OP.Subscribe($"{Module}.{Name}.Continue", (string cmd, object[] param) =>
  392. {
  393. Continue();
  394. return true;
  395. });
  396. if (_scServoMoveSpeed != null)
  397. SetServoMoveSpeed((float)_scServoMoveSpeed.DoubleValue);
  398. if (_scServoAcc != null)
  399. ServoAccSet = (float)_scServoAcc.DoubleValue;
  400. if (_scServoDec != null)
  401. ServoDecSet = (float)_scServoDec.DoubleValue;
  402. if (_doServoOn != null)
  403. _doServoOn.SetValue(true, out _);
  404. return true;
  405. }
  406. public float GetSpeedFromOutputs()
  407. {
  408. if (_diMoving.Value || _diHoming.Value)
  409. {
  410. return GetRAxisSpeed();
  411. }
  412. return 0f;
  413. }
  414. private float GetRAxisSpeed()
  415. {
  416. List<bool> chunks = new List<bool> { _diM0.Value, _diM1.Value, _diM2.Value, _diM3.Value, _diM4.Value, _diM5.Value };
  417. chunks.Reverse();
  418. string binaryString = "";
  419. foreach (bool chunk in chunks)
  420. {
  421. binaryString += chunk ? "1" : "0";
  422. }
  423. int intValue = Convert.ToInt32(binaryString, 2);
  424. if (intValue >= 8 && intValue <= 47)
  425. {
  426. decimal standardSpeed = 0.1M;
  427. for (int i = 8; i < intValue; i++)
  428. {
  429. standardSpeed += 0.1M;
  430. }
  431. return (float)standardSpeed;
  432. }
  433. else
  434. {
  435. return 0f;
  436. }
  437. }
  438. private float GetAxisSpeed()
  439. {
  440. if (IsRAxis)
  441. {
  442. return _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : GetSpeedFromOutputs();
  443. }
  444. else
  445. {
  446. return _aiRealSpeed != null ? (_isFloatAioType ? _aiRealSpeed.FloatValue : _aiRealSpeed.Value) : 0;
  447. }
  448. }
  449. public virtual void Monitor()
  450. {
  451. if (_diNegativeLimit != null)
  452. {
  453. _negativeLimitTrig.CLK = _diNegativeLimit.Value;
  454. if (_negativeLimitTrig.Q)
  455. EV.PostWarningLog(Module, $"{Module} {Name} at negative limit position");
  456. }
  457. if (_diPositiveLimit != null)
  458. {
  459. _positiveLimitTrig.CLK = _diPositiveLimit.Value;
  460. if (_positiveLimitTrig.Q)
  461. EV.PostWarningLog(Module, $"{Module} {Name} at positive limit position");
  462. }
  463. if (_diMotorWarning != null)
  464. {
  465. _warningTrig.CLK = _diMotorWarning.Value;
  466. if (_warningTrig.Q)
  467. EV.PostWarningLog(Module, $"{Module} {Name} warning code={ErrorCode}");
  468. }
  469. if (IsError)
  470. {
  471. _alarmTrig.CLK = IsError;
  472. if (_alarmTrig.Q)
  473. EV.PostWarningLog(Module, $"{Module} {Name} alarm code={ErrorCode}");
  474. }
  475. }
  476. public virtual void Reset()
  477. {
  478. ServoReset(out _);
  479. }
  480. public virtual void Terminate()
  481. {
  482. }
  483. private bool OnTimer()
  484. {
  485. switch (_state)
  486. {
  487. case State.Idle:
  488. if (!_timer.IsIdle())
  489. {
  490. _timer.Stop();
  491. }
  492. _stateTrig.CLK = true;
  493. if (_stateTrig.Q)
  494. ResetDO();
  495. break;
  496. case State.Homing:
  497. if (_timer.GetElapseTime() >= setTime)
  498. {
  499. if (_timer.GetElapseTime() >= moveTimeout)
  500. {
  501. _doHome.SetValue(false, out _);
  502. _state = State.Idle;
  503. EV.PostAlarmLog($"{Module}", $"{Name} Motor home timeout");
  504. }
  505. else
  506. {
  507. if (_diHomeDone != null && _diHomeDone.Value)
  508. {
  509. _doHome.SetValue(false, out _);
  510. _state = State.Idle;
  511. EV.PostInfoLog($"{Module}", $"{Name} Motor home finish");
  512. }
  513. }
  514. }
  515. if (_timer.GetElapseTime() >= resetTime)
  516. {
  517. _doHome.SetValue(false, out _);
  518. }
  519. break;
  520. case State.ServoOff:
  521. if (_timer.GetElapseTime() >= setTime)
  522. {
  523. if (_timer.GetElapseTime() >= moveTimeout)
  524. {
  525. _state = State.Idle;
  526. EV.PostAlarmLog($"{Name}", $"{Name} Motor servo off timeout");
  527. }
  528. else
  529. {
  530. if (_diServoOn != null && !_diServoOn.Value)
  531. {
  532. _state = State.Idle;
  533. }
  534. }
  535. }
  536. break;
  537. case State.ServoOn:
  538. if (_timer.GetElapseTime() >= setTime)
  539. {
  540. if (_timer.GetElapseTime() >= moveTimeout)
  541. {
  542. _state = State.Idle;
  543. EV.PostAlarmLog($"{Module}", $"{Name} Motor on timeout");
  544. }
  545. else
  546. {
  547. if (_diServoOn != null && _diServoOn.Value)
  548. {
  549. _state = State.Idle;
  550. }
  551. }
  552. }
  553. break;
  554. case State.Stopping:
  555. if (_timer.GetElapseTime() >= setTime)
  556. {
  557. if (_timer.GetElapseTime() >= moveTimeout)
  558. {
  559. _doStop.SetValue(false, out _);
  560. _state = State.Idle;
  561. EV.PostAlarmLog($"{Module}", $"{Name} Motor stop timeout");
  562. }
  563. else
  564. {
  565. //if (_diMotorRun != null && !_diMotorRun.Value)
  566. {
  567. _doStop.SetValue(false, out _);
  568. _state = State.Idle;
  569. }
  570. }
  571. }
  572. if (_timer.GetElapseTime() >= resetTime)
  573. {
  574. _doStop.SetValue(false, out _);
  575. }
  576. break;
  577. case State.ServoReset:
  578. if (_timer.GetElapseTime() >= setTime)
  579. {
  580. if (_timer.GetElapseTime() >= moveTimeout)
  581. {
  582. _doReset.SetValue(false, out _);
  583. _state = State.Idle;
  584. EV.PostAlarmLog($"{Module}", $"{Name} Servo reset timeout");
  585. }
  586. if (_timer.GetElapseTime() >= resetTime)
  587. {
  588. _doReset.SetValue(false, out _);
  589. _state = State.Idle;
  590. }
  591. }
  592. break;
  593. case State.MoveToPosition:
  594. var boatModule = Singleton<EquipmentManager>.Instance.Modules[ModuleName.Boat] as BoatModule;
  595. if (!boatModule.CheckPrepareMove(out string reason))
  596. {
  597. boatModule.BoatZAxisMoveFailedForInterlock.Set(reason);
  598. _doMove?.SetValue(false, out _);
  599. _state = State.Idle;
  600. break;
  601. }
  602. if (_timer.GetElapseTime() >= setTime && TargetPositionFb == (int)(ServoMovePositionSet + 0.00001))
  603. {
  604. if (!_doMove.Value && _timer.GetElapseTime() < resetTime)
  605. _doMove.SetValue(true, out _);
  606. if (_timer.GetElapseTime() >= moveTimeout)
  607. {
  608. _doMove.SetValue(false, out _);
  609. _state = State.Idle;
  610. EV.PostAlarmLog($"{Module}", $"{Name} Motor move to position timeout");
  611. }
  612. else if (CheckServoAtPosition())
  613. {
  614. _doMove.SetValue(false, out _);
  615. _state = State.Idle;
  616. EV.PostInfoLog($"{Module}", $"{Name} Motor move to position finish");
  617. }
  618. }
  619. if (_timer.GetElapseTime() >= resetTime)
  620. {
  621. _doMove.SetValue(false, out _);
  622. }
  623. break;
  624. case State.Rotating:
  625. //if (_timer.GetElapseTime() >= 100)
  626. //{
  627. // var currSpeed = GetRAxisSpeed();
  628. // if (!_doMove.Value && _setSpeed == (float)(currSpeed))
  629. // {
  630. // _doCCW.SetValue(!_isCW, out _);
  631. // _doCW.SetValue(_isCW, out _);
  632. // _doMove.SetValue(true, out _);
  633. // SC.SetItemValue(_scServoMoveSpeed.PathName, _setSpeed);
  634. // }
  635. // else
  636. // {
  637. // if (_timer.GetElapseTime() >= resetTime && !_doMove.Value && _setSpeed != (float)(currSpeed))
  638. // {
  639. // EV.PostAlarmLog($"{Module}", $"{Name} Motor Rotating timeout");
  640. // _doMove.SetValue(false, out _);
  641. // _state = State.Idle;
  642. // }
  643. // }
  644. //}
  645. break;
  646. default:
  647. break;
  648. }
  649. return true;
  650. }
  651. public bool SetServoHome()
  652. {
  653. ResetDO();
  654. if (_doHome != null)
  655. {
  656. _doHome?.SetValue(true, out _);
  657. _state = State.Homing;
  658. }
  659. else
  660. {
  661. ServoMoveSpeedSet = (float)_scServoMoveSpeed.DoubleValue;
  662. ServoMovePositionSet = 1;
  663. //_doMove.SetValue(true, out string reason);
  664. _timer.Start(0);
  665. _state = State.MoveToPosition;
  666. }
  667. _timer.Start(0);
  668. LOG.Write($"{Name} set motor home");
  669. return true;
  670. }
  671. private void Continue()
  672. {
  673. _state = State.Idle;
  674. SetServoMoveTo(_lastTarget, out string reason);
  675. }
  676. public bool SetServoMoveTo(string position, out string reason, float speed = 0)
  677. {
  678. reason = string.Empty;
  679. if (_state != State.Idle && _state != State.Rotating)
  680. {
  681. reason = "busy";
  682. return false;
  683. }
  684. ResetDO();
  685. ServoOnOffSet = true;
  686. if (_scServoAcc != null)
  687. ServoAccSet = (float)_scServoAcc.DoubleValue;
  688. if (_scServoDec != null)
  689. ServoDecSet = (float)_scServoDec.DoubleValue;
  690. var target = position;
  691. if (IoMappingDic != null && IoMappingDic.ContainsKey(position))
  692. {
  693. target = IoMappingDic[position];
  694. }
  695. _lastTarget = target;
  696. switch (target)
  697. {
  698. case "Position1":
  699. speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
  700. ServoMoveSpeedSet = speed;
  701. ServoMovePositionSet = 1;
  702. //_doMove.SetValue(true, out reason);
  703. _timer.Start(0);
  704. _state = State.MoveToPosition;
  705. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  706. break;
  707. case "CapPosition":
  708. case "Position2":
  709. speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
  710. ServoMoveSpeedSet = speed;
  711. ServoMovePositionSet = 2;
  712. //_doMove.SetValue(true, out reason);
  713. _timer.Start(0);
  714. _state = State.MoveToPosition;
  715. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  716. break;
  717. case "HomePosition":
  718. case "Position3":
  719. speed = speed > 0 ? speed : (float)_scServoMoveSpeed.DoubleValue;
  720. ServoMoveSpeedSet = speed;
  721. ServoMovePositionSet = 3;
  722. //_doMove.SetValue(true, out reason);
  723. _timer.Start(0);
  724. _state = State.MoveToPosition;
  725. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  726. break;
  727. case "CW":
  728. _state = State.Rotating;
  729. SetRotateSpeed(speed);
  730. _doCCW.SetValue(false, out _);
  731. _doCW.SetValue(true, out _);
  732. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  733. _doMove.SetPulseValue(true, resetTime);
  734. break;
  735. case "CCW":
  736. _state = State.Rotating;
  737. SetRotateSpeed(speed);
  738. _doCCW.SetValue(true, out _);
  739. _doCW.SetValue(false, out _);
  740. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  741. _doMove.SetPulseValue(true, resetTime);
  742. break;
  743. }
  744. return true;
  745. }
  746. private void SetRotateSpeed(float speed)
  747. {
  748. var value = Math.Round(speed, 3, MidpointRounding.AwayFromZero);
  749. decimal standardSpeed = 0.1M;
  750. bool enterSplit = false;
  751. for (int i = 8; i <= 47; i++)
  752. {
  753. decimal compareSpeed = Convert.ToDecimal(value);
  754. if (Decimal.Compare(compareSpeed, standardSpeed) > 0)
  755. {
  756. standardSpeed = standardSpeed + 0.1M;
  757. continue;
  758. }
  759. enterSplit = true;
  760. var binaryString = Convert.ToString(i, 2);
  761. int chunkSize = 1; // 每个分片的大小
  762. List<bool> chunks = new List<bool>();
  763. for (int j = 0; j < binaryString.Length - chunkSize + 1; j += chunkSize)
  764. {
  765. string chunk = binaryString.Substring(j, chunkSize);
  766. chunks.Add(chunk.Equals("1"));
  767. }
  768. chunks.Reverse();
  769. _doM0.SetValue(chunks[0], out _);
  770. _doM1.SetValue(chunks[1], out _);
  771. _doM2.SetValue(chunks[2], out _);
  772. _doM3.SetValue(chunks[3], out _);
  773. switch (chunks.Count)
  774. {
  775. case 5:
  776. _doM4.SetValue(chunks[4], out _);
  777. _doM5.SetValue(false, out _);
  778. break;
  779. case 6:
  780. _doM4.SetValue(chunks[4], out _);
  781. _doM5.SetValue(chunks[5], out _);
  782. break;
  783. default:
  784. _doM4.SetValue(false, out _);
  785. _doM5.SetValue(false, out _);
  786. break;
  787. }
  788. return;
  789. }
  790. if (!enterSplit)
  791. {
  792. _doM0.SetValue(true, out _);
  793. _doM1.SetValue(true, out _);
  794. _doM2.SetValue(false, out _);
  795. _doM3.SetValue(false, out _);
  796. _doM4.SetValue(true, out _);
  797. _doM5.SetValue(false, out _);
  798. }
  799. }
  800. public bool CheckServoAtPosition()
  801. {
  802. int target = (int)(_aoTargetPosition.FloatValue + 0.0001);
  803. switch (target)
  804. {
  805. case 1:
  806. return _diPosition1.Value;
  807. case 2:
  808. return _diPosition2.Value;
  809. case 3:
  810. return _diPosition3.Value;
  811. }
  812. return false;
  813. }
  814. public bool CheckServoAtPosition(string position)
  815. {
  816. switch (position)
  817. {
  818. case "Position1":
  819. return _diPosition1.Value;
  820. case "Position2":
  821. case "CapPosition":
  822. return _diPosition2.Value;
  823. case "HomePosition":
  824. case "Position3":
  825. return _diPosition3.Value;
  826. }
  827. return false;
  828. }
  829. public bool ServoReset(out string reason)
  830. {
  831. reason = string.Empty;
  832. _doReset.SetPulseValue(true, resetTime);
  833. return true;
  834. }
  835. public void SetServoOnOff(bool isOn)
  836. {
  837. ServoOnOffSet = isOn;
  838. }
  839. public void ServoStop()
  840. {
  841. ResetDO();
  842. _state = State.Idle;
  843. _doStop.SetPulseValue(true, resetTime);
  844. }
  845. public void SetSpeed(float value)
  846. {
  847. SetServoPara("MoveSpeed", value);
  848. }
  849. public void SetPauseResume(bool isPause)
  850. {
  851. IsPause = IsPause;
  852. }
  853. private void SetServoPara(string module, float value)
  854. {
  855. var target = module;
  856. if (IoMappingDic != null && IoMappingDic.ContainsKey(module))
  857. {
  858. target = IoMappingDic[module];
  859. }
  860. switch (target)
  861. {
  862. case "MoveSpeed":
  863. SetServoMoveSpeed(value);
  864. break;
  865. case "Position1":
  866. SC.SetItemValue(_scServoPosition1.PathName, value);
  867. break;
  868. case "Position2":
  869. SC.SetItemValue(_scServoPosition2.PathName, value);
  870. break;
  871. case "Position3":
  872. SC.SetItemValue(_scServoPosition3.PathName, value);
  873. break;
  874. case "Position4":
  875. SC.SetItemValue(_scServoPosition4.PathName, value);
  876. break;
  877. case "Position5":
  878. SC.SetItemValue(_scServoPosition5.PathName, value);
  879. break;
  880. case "Position6":
  881. SC.SetItemValue(_scServoPosition6.PathName, value);
  882. break;
  883. case "Position7":
  884. SC.SetItemValue(_scServoPosition7.PathName, value);
  885. break;
  886. case "Position8":
  887. SC.SetItemValue(_scServoPosition8.PathName, value);
  888. break;
  889. case "Position9":
  890. SC.SetItemValue(_scServoPosition9.PathName, value);
  891. break;
  892. case "Position10":
  893. SC.SetItemValue(_scServoPosition10.PathName, value);
  894. break;
  895. }
  896. }
  897. private void SetServoMoveSpeed(float speed)
  898. {
  899. ServoMoveSpeedSet = speed;
  900. SC.SetItemValue(_scServoMoveSpeed.PathName, speed);
  901. }
  902. private void SetServoMovePosition(float position)
  903. {
  904. ServoMovePositionSet = position;
  905. }
  906. protected void ResetDO()
  907. {
  908. if (_doHome != null && _doHome.Value)
  909. _doHome.SetValue(false, out _);
  910. if (_doInit != null && _doInit.Value)
  911. _doInit.SetValue(false, out _);
  912. if (_doMove != null && _doMove.Value)
  913. _doMove.SetValue(false, out _);
  914. if (_doStop != null && _doStop.Value)
  915. _doStop.SetValue(false, out _);
  916. if (_doCW != null && _doCW.Value)
  917. _doCW.SetValue(false, out _);
  918. if (_doCCW != null && _doCCW.Value)
  919. _doCCW.SetValue(false, out _);
  920. if (_doReset != null && _doReset.Value)
  921. _doReset.SetValue(false, out _);
  922. }
  923. }
  924. }