IoFurnaceMotor.cs 38 KB

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