NcdVehicle.cs 64 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688
  1. using Aitex.Core.RT.DataCenter;
  2. using Aitex.Core.RT.Device;
  3. using Aitex.Core.RT.Event;
  4. using Aitex.Core.RT.Fsm;
  5. using Aitex.Core.RT.IOCore;
  6. using Aitex.Core.RT.Routine;
  7. using Aitex.Core.Util;
  8. using MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Vehicle;
  9. using System;
  10. using System.Collections.Generic;
  11. using System.Linq;
  12. using System.Text;
  13. using System.Threading.Tasks;
  14. using System.Xml;
  15. namespace Aitex.Core.RT.Device.Unit
  16. {
  17. public class NcdVehicle: VehicleBaseDevice
  18. {
  19. public NcdVehicle(string module, XmlElement node, string ioModule = "") : base(node.GetAttribute("module"), node.GetAttribute("id"))
  20. {
  21. base.Module = node.GetAttribute("module");//string.IsNullOrEmpty(node.GetAttribute("module")) ? module : node.GetAttribute("module");
  22. base.Name = node.GetAttribute("id");
  23. ioModule = node.GetAttribute("ioModule");
  24. _aiVehiclePLCState = ParseAiNode($"AI_VehiclePLCState", node, ioModule);
  25. _aiTargetPosition = ParseAiNode($"AI_TargetPosition", node, ioModule);
  26. _aiCurrentCoordinate_L = ParseAiNode($"AI_CurrentCoordinate_L", node, ioModule);
  27. _aiCurrentCoordinate_H = ParseAiNode($"AI_CurrentCoordinate_H", node, ioModule);
  28. _aiVehiclePLCIP = ParseAiNode($"AI_VehiclePLCIP", node, ioModule);
  29. _aiVehicleID0 = ParseAiNode($"AI_VehicleID0", node, ioModule);
  30. _aiVehicleID1 = ParseAiNode($"AI_VehicleID1", node, ioModule);
  31. _aiVehicleID2 = ParseAiNode($"AI_VehicleID2", node, ioModule);
  32. _aiVehicleID3 = ParseAiNode($"AI_VehicleID3", node, ioModule);
  33. _aiCurrentCommand = ParseAiNode($"AI_CurrentCommand", node, ioModule);
  34. _aiCurrentCommandStatus = ParseAiNode($"AI_CurrentCommandStatus", node, ioModule);
  35. _aiErrorCode = ParseAiNode($"AI_ErrorCode", node, ioModule);
  36. _aiDistanceToFrontVehicle = ParseAiNode($"AI_DistanceToFrontVehicle", node, ioModule);
  37. _aiRFIDChar0 = ParseAiNode($"AI_RFIDChar0", node, ioModule);
  38. _aiRFIDChar1 = ParseAiNode($"AI_RFIDChar1", node, ioModule);
  39. _aiRFIDChar2 = ParseAiNode($"AI_RFIDChar2", node, ioModule);
  40. _aiRFIDChar3 = ParseAiNode($"AI_RFIDChar3", node, ioModule);
  41. _aiRFIDChar4 = ParseAiNode($"AI_RFIDChar4", node, ioModule);
  42. _aiRFIDChar5 = ParseAiNode($"AI_RFIDChar5", node, ioModule);
  43. _aiRFIDChar6 = ParseAiNode($"AI_RFIDChar6", node, ioModule);
  44. _aiRFIDChar7 = ParseAiNode($"AI_RFIDChar7", node, ioModule);
  45. _aiCurrentPosition = ParseAiNode($"AI_CurrentPosition", node, ioModule);
  46. _aoVehicleCommand = ParseAoNode($"AO_VehicleCommand", node, ioModule);
  47. _aoTargetPosition = ParseAoNode($"AO_TargetPosition", node, ioModule);
  48. _aoViaCoordinate1_L = ParseAoNode($"AO_ViaCoordinate1_L", node, ioModule);
  49. _aoViaCoordinate1_H = ParseAoNode($"AO_ViaCoordinate1_H", node, ioModule);
  50. _aoViaCoordinate2_L = ParseAoNode($"AO_ViaCoordinate2_L", node, ioModule);
  51. _aoViaCoordinate2_H = ParseAoNode($"AO_ViaCoordinate2_H", node, ioModule);
  52. _aoViaCoordinate3_L = ParseAoNode($"AO_ViaCoordinate3_L", node, ioModule);
  53. _aoViaCoordinate3_H = ParseAoNode($"AO_ViaCoordinate3_H", node, ioModule);
  54. _aoViaCoordinate4_L = ParseAoNode($"AO_ViaCoordinate4_L", node, ioModule);
  55. _aoViaCoordinate4_H = ParseAoNode($"AO_ViaCoordinate4_H", node, ioModule);
  56. _aoViaCoordinate5_L = ParseAoNode($"AO_ViaCoordinate5_L", node, ioModule);
  57. _aoViaCoordinate5_H = ParseAoNode($"AO_ViaCoordinate5_H", node, ioModule);
  58. _aoViaCoordinate6_L = ParseAoNode($"AO_ViaCoordinate6_L", node, ioModule);
  59. _aoViaCoordinate6_H = ParseAoNode($"AO_ViaCoordinate6_H", node, ioModule);
  60. _aoViaCoordinate7_L = ParseAoNode($"AO_ViaCoordinate7_L", node, ioModule);
  61. _aoViaCoordinate7_H = ParseAoNode($"AO_ViaCoordinate7_H", node, ioModule);
  62. _aoViaCoordinate8_L = ParseAoNode($"AO_ViaCoordinate8_L", node, ioModule);
  63. _aoViaCoordinate8_H = ParseAoNode($"AO_ViaCoordinate8_H", node, ioModule);
  64. _aoViaCoordinate9_L = ParseAoNode($"AO_ViaCoordinate9_L", node, ioModule);
  65. _aoViaCoordinate9_H = ParseAoNode($"AO_ViaCoordinate9_H", node, ioModule);
  66. _aoViaCoordinate10_L = ParseAoNode($"AO_ViaCoordinate10_L", node, ioModule);
  67. _aoViaCoordinate10_H = ParseAoNode($"AO_ViaCoordinate10_H", node, ioModule);
  68. _aoViaCoordinate11_L = ParseAoNode($"AO_ViaCoordinate11_L", node, ioModule);
  69. _aoViaCoordinate11_H = ParseAoNode($"AO_ViaCoordinate11_H", node, ioModule);
  70. _aoViaCoordinate12_L = ParseAoNode($"AO_ViaCoordinate12_L", node, ioModule);
  71. _aoViaCoordinate12_H = ParseAoNode($"AO_ViaCoordinate12_H", node, ioModule);
  72. _aoViaCoordinate13_L = ParseAoNode($"AO_ViaCoordinate13_L", node, ioModule);
  73. _aoViaCoordinate13_H = ParseAoNode($"AO_ViaCoordinate13_H", node, ioModule);
  74. _aoViaCoordinate14_L = ParseAoNode($"AO_ViaCoordinate14_L", node, ioModule);
  75. _aoViaCoordinate14_H = ParseAoNode($"AO_ViaCoordinate14_H", node, ioModule);
  76. _aoViaCoordinate15_L = ParseAoNode($"AO_ViaCoordinate15_L", node, ioModule);
  77. _aoViaCoordinate15_H = ParseAoNode($"AO_ViaCoordinate15_H", node, ioModule);
  78. _aoViaCoordinate16_L = ParseAoNode($"AO_ViaCoordinate16_L", node, ioModule);
  79. _aoViaCoordinate16_H = ParseAoNode($"AO_ViaCoordinate16_H", node, ioModule);
  80. _aoViaCoordinate17_L = ParseAoNode($"AO_ViaCoordinate17_L", node, ioModule);
  81. _aoViaCoordinate17_H = ParseAoNode($"AO_ViaCoordinate17_H", node, ioModule);
  82. _aoViaCoordinate18_L = ParseAoNode($"AO_ViaCoordinate18_L", node, ioModule);
  83. _aoViaCoordinate18_H = ParseAoNode($"AO_ViaCoordinate18_H", node, ioModule);
  84. _aoViaCoordinate19_L = ParseAoNode($"AO_ViaCoordinate19_L", node, ioModule);
  85. _aoViaCoordinate19_H = ParseAoNode($"AO_ViaCoordinate19_H", node, ioModule);
  86. _aoViaCoordinate20_L = ParseAoNode($"AO_ViaCoordinate20_L", node, ioModule);
  87. _aoViaCoordinate20_H = ParseAoNode($"AO_ViaCoordinate20_H", node, ioModule);
  88. _aoViaCoordinate21_L = ParseAoNode($"AO_ViaCoordinate21_L", node, ioModule);
  89. _aoViaCoordinate21_H = ParseAoNode($"AO_ViaCoordinate21_H", node, ioModule);
  90. _aoViaCoordinate22_L = ParseAoNode($"AO_ViaCoordinate22_L", node, ioModule);
  91. _aoViaCoordinate22_H = ParseAoNode($"AO_ViaCoordinate22_H", node, ioModule);
  92. _aoViaCoordinate23_L = ParseAoNode($"AO_ViaCoordinate23_L", node, ioModule);
  93. _aoViaCoordinate23_H = ParseAoNode($"AO_ViaCoordinate23_H", node, ioModule);
  94. _aoViaCoordinate24_L = ParseAoNode($"AO_ViaCoordinate24_L", node, ioModule);
  95. _aoViaCoordinate24_H = ParseAoNode($"AO_ViaCoordinate24_H", node, ioModule);
  96. _aoViaCoordinate25_L = ParseAoNode($"AO_ViaCoordinate25_L", node, ioModule);
  97. _aoViaCoordinate25_H = ParseAoNode($"AO_ViaCoordinate25_H", node, ioModule);
  98. _aoViaCoordinate26_L = ParseAoNode($"AO_ViaCoordinate26_L", node, ioModule);
  99. _aoViaCoordinate26_H = ParseAoNode($"AO_ViaCoordinate26_H", node, ioModule);
  100. _aoViaCoordinate27_L = ParseAoNode($"AO_ViaCoordinate27_L", node, ioModule);
  101. _aoViaCoordinate27_H = ParseAoNode($"AO_ViaCoordinate27_H", node, ioModule);
  102. _aoViaCoordinate28_L = ParseAoNode($"AO_ViaCoordinate28_L", node, ioModule);
  103. _aoViaCoordinate28_H = ParseAoNode($"AO_ViaCoordinate28_H", node, ioModule);
  104. _aoViaCoordinate29_L = ParseAoNode($"AO_ViaCoordinate29_L", node, ioModule);
  105. _aoViaCoordinate29_H = ParseAoNode($"AO_ViaCoordinate29_H", node, ioModule);
  106. _aoViaCoordinate30_L = ParseAoNode($"AO_ViaCoordinate30_L", node, ioModule);
  107. _aoViaCoordinate30_H = ParseAoNode($"AO_ViaCoordinate30_H", node, ioModule);
  108. _aoViaCoordinate31_L = ParseAoNode($"AO_ViaCoordinate31_L", node, ioModule);
  109. _aoViaCoordinate31_H = ParseAoNode($"AO_ViaCoordinate31_H", node, ioModule);
  110. _aoViaCoordinate32_L = ParseAoNode($"AO_ViaCoordinate32_L", node, ioModule);
  111. _aoViaCoordinate32_H = ParseAoNode($"AO_ViaCoordinate32_H", node, ioModule);
  112. _aoViaCoordinate33_L = ParseAoNode($"AO_ViaCoordinate33_L", node, ioModule);
  113. _aoViaCoordinate33_H = ParseAoNode($"AO_ViaCoordinate33_H", node, ioModule);
  114. _aoViaCoordinate34_L = ParseAoNode($"AO_ViaCoordinate34_L", node, ioModule);
  115. _aoViaCoordinate34_H = ParseAoNode($"AO_ViaCoordinate34_H", node, ioModule);
  116. _aoViaCoordinate35_L = ParseAoNode($"AO_ViaCoordinate35_L", node, ioModule);
  117. _aoViaCoordinate35_H = ParseAoNode($"AO_ViaCoordinate35_H", node, ioModule);
  118. _aoViaCoordinate36_L = ParseAoNode($"AO_ViaCoordinate36_L", node, ioModule);
  119. _aoViaCoordinate36_H = ParseAoNode($"AO_ViaCoordinate36_H", node, ioModule);
  120. _aoViaCoordinate37_L = ParseAoNode($"AO_ViaCoordinate37_L", node, ioModule);
  121. _aoViaCoordinate37_H = ParseAoNode($"AO_ViaCoordinate37_H", node, ioModule);
  122. _aoViaCoordinate38_L = ParseAoNode($"AO_ViaCoordinate38_L", node, ioModule);
  123. _aoViaCoordinate38_H = ParseAoNode($"AO_ViaCoordinate38_H", node, ioModule);
  124. _aoViaCoordinate39_L = ParseAoNode($"AO_ViaCoordinate39_L", node, ioModule);
  125. _aoViaCoordinate39_H = ParseAoNode($"AO_ViaCoordinate39_H", node, ioModule);
  126. _aoViaCoordinate40_L = ParseAoNode($"AO_ViaCoordinate40_L", node, ioModule);
  127. _aoViaCoordinate40_H = ParseAoNode($"AO_ViaCoordinate40_H", node, ioModule);
  128. _aoViaCoordinate41_L = ParseAoNode($"AO_ViaCoordinate41_L", node, ioModule);
  129. _aoViaCoordinate41_H = ParseAoNode($"AO_ViaCoordinate41_H", node, ioModule);
  130. _aoViaCoordinate42_L = ParseAoNode($"AO_ViaCoordinate42_L", node, ioModule);
  131. _aoViaCoordinate42_H = ParseAoNode($"AO_ViaCoordinate42_H", node, ioModule);
  132. _aoViaCoordinate43_L = ParseAoNode($"AO_ViaCoordinate43_L", node, ioModule);
  133. _aoViaCoordinate43_H = ParseAoNode($"AO_ViaCoordinate43_H", node, ioModule);
  134. _aoViaCoordinate44_L = ParseAoNode($"AO_ViaCoordinate44_L", node, ioModule);
  135. _aoViaCoordinate44_H = ParseAoNode($"AO_ViaCoordinate44_H", node, ioModule);
  136. _aoViaCoordinate45_L = ParseAoNode($"AO_ViaCoordinate45_L", node, ioModule);
  137. _aoViaCoordinate45_H = ParseAoNode($"AO_ViaCoordinate45_H", node, ioModule);
  138. _aoViaCoordinate46_L = ParseAoNode($"AO_ViaCoordinate46_L", node, ioModule);
  139. _aoViaCoordinate46_H = ParseAoNode($"AO_ViaCoordinate46_H", node, ioModule);
  140. _aoViaCoordinate47_L = ParseAoNode($"AO_ViaCoordinate47_L", node, ioModule);
  141. _aoViaCoordinate47_H = ParseAoNode($"AO_ViaCoordinate47_H", node, ioModule);
  142. _aoViaCoordinate48_L = ParseAoNode($"AO_ViaCoordinate48_L", node, ioModule);
  143. _aoViaCoordinate48_H = ParseAoNode($"AO_ViaCoordinate48_H", node, ioModule);
  144. _aoViaCoordinate49_L = ParseAoNode($"AO_ViaCoordinate49_L", node, ioModule);
  145. _aoViaCoordinate49_H = ParseAoNode($"AO_ViaCoordinate49_H", node, ioModule);
  146. _aoViaCoordinate50_L = ParseAoNode($"AO_ViaCoordinate50_L", node, ioModule);
  147. _aoViaCoordinate50_H = ParseAoNode($"AO_ViaCoordinate50_H", node, ioModule);
  148. _aoViaCoordinates = new AOAccessor[]
  149. {
  150. _aoViaCoordinate1_L,
  151. _aoViaCoordinate1_H,
  152. _aoViaCoordinate2_L,
  153. _aoViaCoordinate2_H,
  154. _aoViaCoordinate3_L,
  155. _aoViaCoordinate3_H,
  156. _aoViaCoordinate4_L,
  157. _aoViaCoordinate4_H,
  158. _aoViaCoordinate5_L,
  159. _aoViaCoordinate5_H,
  160. _aoViaCoordinate6_L,
  161. _aoViaCoordinate6_H,
  162. _aoViaCoordinate7_L,
  163. _aoViaCoordinate7_H,
  164. _aoViaCoordinate8_L,
  165. _aoViaCoordinate8_H,
  166. _aoViaCoordinate9_L,
  167. _aoViaCoordinate9_H,
  168. _aoViaCoordinate10_L,
  169. _aoViaCoordinate10_H,
  170. _aoViaCoordinate11_L,
  171. _aoViaCoordinate11_H,
  172. _aoViaCoordinate12_L,
  173. _aoViaCoordinate12_H,
  174. _aoViaCoordinate13_L,
  175. _aoViaCoordinate13_H,
  176. _aoViaCoordinate14_L,
  177. _aoViaCoordinate14_H,
  178. _aoViaCoordinate15_L,
  179. _aoViaCoordinate15_H,
  180. _aoViaCoordinate16_L,
  181. _aoViaCoordinate16_H,
  182. _aoViaCoordinate17_L,
  183. _aoViaCoordinate17_H,
  184. _aoViaCoordinate18_L,
  185. _aoViaCoordinate18_H,
  186. _aoViaCoordinate19_L,
  187. _aoViaCoordinate19_H,
  188. _aoViaCoordinate20_L,
  189. _aoViaCoordinate20_H,
  190. _aoViaCoordinate21_L,
  191. _aoViaCoordinate21_H,
  192. _aoViaCoordinate22_L,
  193. _aoViaCoordinate22_H,
  194. _aoViaCoordinate23_L,
  195. _aoViaCoordinate23_H,
  196. _aoViaCoordinate24_L,
  197. _aoViaCoordinate24_H,
  198. _aoViaCoordinate25_L,
  199. _aoViaCoordinate25_H,
  200. _aoViaCoordinate26_L,
  201. _aoViaCoordinate26_H,
  202. _aoViaCoordinate27_L,
  203. _aoViaCoordinate27_H,
  204. _aoViaCoordinate28_L,
  205. _aoViaCoordinate28_H,
  206. _aoViaCoordinate29_L,
  207. _aoViaCoordinate29_H,
  208. _aoViaCoordinate30_L,
  209. _aoViaCoordinate30_H,
  210. _aoViaCoordinate31_L,
  211. _aoViaCoordinate31_H,
  212. _aoViaCoordinate32_L,
  213. _aoViaCoordinate32_H,
  214. _aoViaCoordinate33_L,
  215. _aoViaCoordinate33_H,
  216. _aoViaCoordinate34_L,
  217. _aoViaCoordinate34_H,
  218. _aoViaCoordinate35_L,
  219. _aoViaCoordinate35_H,
  220. _aoViaCoordinate36_L,
  221. _aoViaCoordinate36_H,
  222. _aoViaCoordinate37_L,
  223. _aoViaCoordinate37_H,
  224. _aoViaCoordinate38_L,
  225. _aoViaCoordinate38_H,
  226. _aoViaCoordinate39_L,
  227. _aoViaCoordinate39_H,
  228. _aoViaCoordinate40_L,
  229. _aoViaCoordinate40_H,
  230. _aoViaCoordinate41_L,
  231. _aoViaCoordinate41_H,
  232. _aoViaCoordinate42_L,
  233. _aoViaCoordinate42_H,
  234. _aoViaCoordinate43_L,
  235. _aoViaCoordinate43_H,
  236. _aoViaCoordinate44_L,
  237. _aoViaCoordinate44_H,
  238. _aoViaCoordinate45_L,
  239. _aoViaCoordinate45_H,
  240. _aoViaCoordinate46_L,
  241. _aoViaCoordinate46_H,
  242. _aoViaCoordinate47_L,
  243. _aoViaCoordinate47_H,
  244. _aoViaCoordinate48_L,
  245. _aoViaCoordinate48_H,
  246. _aoViaCoordinate49_L,
  247. _aoViaCoordinate49_H,
  248. _aoViaCoordinate50_L,
  249. _aoViaCoordinate50_H,
  250. };
  251. SubscribeDataVariable();
  252. }
  253. private AIAccessor _aiVehiclePLCState;
  254. private AIAccessor _aiTargetPosition;
  255. private AIAccessor _aiCurrentCoordinate_L;
  256. private AIAccessor _aiCurrentCoordinate_H;
  257. private AIAccessor _aiVehiclePLCIP;
  258. private AIAccessor _aiVehicleID0;
  259. private AIAccessor _aiVehicleID1;
  260. private AIAccessor _aiVehicleID2;
  261. private AIAccessor _aiVehicleID3;
  262. private AIAccessor _aiCurrentCommand;
  263. private AIAccessor _aiCurrentCommandStatus;
  264. private AIAccessor _aiErrorCode;
  265. private AIAccessor _aiDistanceToFrontVehicle;
  266. private AIAccessor _aiTeachCommandStatus;
  267. private AIAccessor _aiCurrentXPosition_L;
  268. private AIAccessor _aiCurrentXPosition_H;
  269. private AIAccessor _aiCurrentYPosition_L;
  270. private AIAccessor _aiCurrentYPosition_H;
  271. private AIAccessor _aiCurrentZPosition_L;
  272. private AIAccessor _aiCurrentZPosition_H;
  273. private AIAccessor _aiRFIDChar0;
  274. private AIAccessor _aiRFIDChar1;
  275. private AIAccessor _aiRFIDChar2;
  276. private AIAccessor _aiRFIDChar3;
  277. private AIAccessor _aiRFIDChar4;
  278. private AIAccessor _aiRFIDChar5;
  279. private AIAccessor _aiRFIDChar6;
  280. private AIAccessor _aiRFIDChar7;
  281. private AIAccessor _aiRFIDChar8;
  282. private AIAccessor _aiRFIDChar9;
  283. private AIAccessor _aiRFIDChar10;
  284. private AIAccessor _aiCurrentPosition;
  285. private AOAccessor _aoVehicleCommand;
  286. private AOAccessor _aoTargetPosition;
  287. private AOAccessor _aoViaCoordinate1_L;
  288. private AOAccessor _aoViaCoordinate1_H;
  289. private AOAccessor _aoViaCoordinate2_L;
  290. private AOAccessor _aoViaCoordinate2_H;
  291. private AOAccessor _aoViaCoordinate3_L;
  292. private AOAccessor _aoViaCoordinate3_H;
  293. private AOAccessor _aoViaCoordinate4_L;
  294. private AOAccessor _aoViaCoordinate4_H;
  295. private AOAccessor _aoViaCoordinate5_L;
  296. private AOAccessor _aoViaCoordinate5_H;
  297. private AOAccessor _aoViaCoordinate6_L;
  298. private AOAccessor _aoViaCoordinate6_H;
  299. private AOAccessor _aoViaCoordinate7_L;
  300. private AOAccessor _aoViaCoordinate7_H;
  301. private AOAccessor _aoViaCoordinate8_L;
  302. private AOAccessor _aoViaCoordinate8_H;
  303. private AOAccessor _aoViaCoordinate9_L;
  304. private AOAccessor _aoViaCoordinate9_H;
  305. private AOAccessor _aoViaCoordinate10_L;
  306. private AOAccessor _aoViaCoordinate10_H;
  307. private AOAccessor _aoViaCoordinate11_L;
  308. private AOAccessor _aoViaCoordinate11_H;
  309. private AOAccessor _aoViaCoordinate12_L;
  310. private AOAccessor _aoViaCoordinate12_H;
  311. private AOAccessor _aoViaCoordinate13_L;
  312. private AOAccessor _aoViaCoordinate13_H;
  313. private AOAccessor _aoViaCoordinate14_L;
  314. private AOAccessor _aoViaCoordinate14_H;
  315. private AOAccessor _aoViaCoordinate15_L;
  316. private AOAccessor _aoViaCoordinate15_H;
  317. private AOAccessor _aoViaCoordinate16_L;
  318. private AOAccessor _aoViaCoordinate16_H;
  319. private AOAccessor _aoViaCoordinate17_L;
  320. private AOAccessor _aoViaCoordinate17_H;
  321. private AOAccessor _aoViaCoordinate18_L;
  322. private AOAccessor _aoViaCoordinate18_H;
  323. private AOAccessor _aoViaCoordinate19_L;
  324. private AOAccessor _aoViaCoordinate19_H;
  325. private AOAccessor _aoViaCoordinate20_L;
  326. private AOAccessor _aoViaCoordinate20_H;
  327. private AOAccessor _aoViaCoordinate21_L;
  328. private AOAccessor _aoViaCoordinate21_H;
  329. private AOAccessor _aoViaCoordinate22_L;
  330. private AOAccessor _aoViaCoordinate22_H;
  331. private AOAccessor _aoViaCoordinate23_L;
  332. private AOAccessor _aoViaCoordinate23_H;
  333. private AOAccessor _aoViaCoordinate24_L;
  334. private AOAccessor _aoViaCoordinate24_H;
  335. private AOAccessor _aoViaCoordinate25_L;
  336. private AOAccessor _aoViaCoordinate25_H;
  337. private AOAccessor _aoViaCoordinate26_L;
  338. private AOAccessor _aoViaCoordinate26_H;
  339. private AOAccessor _aoViaCoordinate27_L;
  340. private AOAccessor _aoViaCoordinate27_H;
  341. private AOAccessor _aoViaCoordinate28_L;
  342. private AOAccessor _aoViaCoordinate28_H;
  343. private AOAccessor _aoViaCoordinate29_L;
  344. private AOAccessor _aoViaCoordinate29_H;
  345. private AOAccessor _aoViaCoordinate30_L;
  346. private AOAccessor _aoViaCoordinate30_H;
  347. private AOAccessor _aoViaCoordinate31_L;
  348. private AOAccessor _aoViaCoordinate31_H;
  349. private AOAccessor _aoViaCoordinate32_L;
  350. private AOAccessor _aoViaCoordinate32_H;
  351. private AOAccessor _aoViaCoordinate33_L;
  352. private AOAccessor _aoViaCoordinate33_H;
  353. private AOAccessor _aoViaCoordinate34_L;
  354. private AOAccessor _aoViaCoordinate34_H;
  355. private AOAccessor _aoViaCoordinate35_L;
  356. private AOAccessor _aoViaCoordinate35_H;
  357. private AOAccessor _aoViaCoordinate36_L;
  358. private AOAccessor _aoViaCoordinate36_H;
  359. private AOAccessor _aoViaCoordinate37_L;
  360. private AOAccessor _aoViaCoordinate37_H;
  361. private AOAccessor _aoViaCoordinate38_L;
  362. private AOAccessor _aoViaCoordinate38_H;
  363. private AOAccessor _aoViaCoordinate39_L;
  364. private AOAccessor _aoViaCoordinate39_H;
  365. private AOAccessor _aoViaCoordinate40_L;
  366. private AOAccessor _aoViaCoordinate40_H;
  367. private AOAccessor _aoViaCoordinate41_L;
  368. private AOAccessor _aoViaCoordinate41_H;
  369. private AOAccessor _aoViaCoordinate42_L;
  370. private AOAccessor _aoViaCoordinate42_H;
  371. private AOAccessor _aoViaCoordinate43_L;
  372. private AOAccessor _aoViaCoordinate43_H;
  373. private AOAccessor _aoViaCoordinate44_L;
  374. private AOAccessor _aoViaCoordinate44_H;
  375. private AOAccessor _aoViaCoordinate45_L;
  376. private AOAccessor _aoViaCoordinate45_H;
  377. private AOAccessor _aoViaCoordinate46_L;
  378. private AOAccessor _aoViaCoordinate46_H;
  379. private AOAccessor _aoViaCoordinate47_L;
  380. private AOAccessor _aoViaCoordinate47_H;
  381. private AOAccessor _aoViaCoordinate48_L;
  382. private AOAccessor _aoViaCoordinate48_H;
  383. private AOAccessor _aoViaCoordinate49_L;
  384. private AOAccessor _aoViaCoordinate49_H;
  385. private AOAccessor _aoViaCoordinate50_L;
  386. private AOAccessor _aoViaCoordinate50_H;
  387. private AOAccessor[] _aoViaCoordinates;
  388. private bool _stateIsInitilizing => (_aiVehiclePLCState.Value & 0x1) == 0x1;
  389. private bool _stateIsIdle => (_aiVehiclePLCState.Value & 0x2) == 0x2;
  390. private bool _stateIsMoving => (_aiVehiclePLCState.Value & 0x4) == 0x4;
  391. private bool _stateIsPicking => (_aiVehiclePLCState.Value & 0x8) == 0x8;
  392. private bool _stateIsPlacing => (_aiVehiclePLCState.Value & 0x10) == 0x10;
  393. private bool _stateIsPaused => (_aiVehiclePLCState.Value & 0x20) == 0x20;
  394. private bool _stateIsWait => (_aiVehiclePLCState.Value & 0x40) == 0x40;
  395. private bool _stateIsOnlineMode => (_aiVehiclePLCState.Value & 0x80) == 0x80;
  396. private bool _stateIsTeachMode => (_aiVehiclePLCState.Value & 0x100) == 0x100;
  397. private bool _stateIsError => (_aiVehiclePLCState.Value & 0x200) == 0x200;
  398. private uint _vehicleTargePostion => BitConverter.ToUInt16(BitConverter.GetBytes(_aiTargetPosition.Value),0);
  399. private float _vehicleCurrentCoordinate
  400. {
  401. get
  402. {
  403. List<byte> value = new List<byte>();
  404. value.AddRange(BitConverter.GetBytes(_aiCurrentCoordinate_L.Value));
  405. value.AddRange(BitConverter.GetBytes(_aiCurrentCoordinate_H.Value));
  406. return BitConverter.ToSingle(value.ToArray(), 0);
  407. }
  408. }
  409. private uint _vehicleCurrentPosition => BitConverter.ToUInt16(BitConverter.GetBytes(_aiCurrentPosition.Value), 0);
  410. private string _vehicleIdOnPlc
  411. {
  412. get
  413. {
  414. List<byte> lbName = new List<byte>();
  415. lbName.AddRange(BitConverter.GetBytes(_aiVehicleID0.Value));
  416. lbName.AddRange(BitConverter.GetBytes(_aiVehicleID1.Value));
  417. lbName.AddRange(BitConverter.GetBytes(_aiVehicleID2.Value));
  418. lbName.AddRange(BitConverter.GetBytes(_aiVehicleID3.Value));
  419. return Encoding.ASCII.GetString(lbName.ToArray());
  420. }
  421. }
  422. private uint _vehiclePLCIP => BitConverter.ToUInt16(BitConverter.GetBytes(_aiVehiclePLCIP.Value), 0);
  423. //private bool _currentCmdIsReplyInit => (_aiCurrentCommand.Value & 0x1) == 0x1;
  424. private bool _currentCmdIsReplyPause => (_aiCurrentCommand.Value & 0x1) == 0x1;
  425. private bool _currentCmdIsReplyClearCommand => (_aiCurrentCommand.Value & 0x2) == 0x2;
  426. private bool _currentCmdIsReplySetOnline => (_aiCurrentCommand.Value & 0x4) == 0x4;
  427. private bool _currentCmdIsReplyMoveToPosition => (_aiCurrentCommand.Value & 0x8) == 0x8;
  428. private bool _currentCmdIsReplyPick => (_aiCurrentCommand.Value & 0x10) == 0x10;
  429. private bool _currentCmdIsReplyPlace => (_aiCurrentCommand.Value & 0x20) == 0x20;
  430. private bool _currentCmdIsReplyChangeRoute => (_aiCurrentCommand.Value & 0x40) == 0x40;
  431. private bool _currentCmdIsReplyReadRFID => (_aiCurrentCommand.Value & 0x80) == 0x80;
  432. private bool _currentCmdIsReplyClearError => (_aiCurrentCommand.Value & 0x100) == 0x100;
  433. private bool _currentCmdIsReplyInit => (_aiCurrentCommand.Value & 0x200) == 0x200;
  434. private bool _cmdResultIsACK => (_aiCurrentCommandStatus.Value & 0x1) == 0x1;
  435. private bool _cmdResultIsComplete => (_aiCurrentCommandStatus.Value & 0x2) == 0x2;
  436. private bool _cmdResultIsNAK => (_aiCurrentCommandStatus.Value & 0x4) == 0x4;
  437. private bool _cmdResultIsAbs => (_aiCurrentCommandStatus.Value & 0x8) == 0x8;
  438. private bool _cmdResultIsRfidOK => (_aiCurrentCommandStatus.Value & 0x10) == 0x10;
  439. private bool _cmdResultIsRfidNG => (_aiCurrentCommandStatus.Value & 0x20) == 0x20;
  440. private bool _currentCmdIsSendPause => (_aoVehicleCommand.Value & 0x1) == 0x1;
  441. private bool _currentCmdIsSendClearCommand => (_aoVehicleCommand.Value & 0x2) == 0x2;
  442. private bool _currentCmdIsSendSetOnline => (_aoVehicleCommand.Value & 0x4) == 0x4;
  443. private bool _currentCmdIsSendMoveToPosition => (_aoVehicleCommand.Value & 0x8) == 0x8;
  444. private bool _currentCmdIsSendPick => (_aoVehicleCommand.Value & 0x10) == 0x10;
  445. private bool _currentCmdIsSendPlace => (_aoVehicleCommand.Value & 0x20) == 0x20;
  446. private bool _currentCmdIsSendChangeRoute => (_aoVehicleCommand.Value & 0x40) == 0x40;
  447. private bool _currentCmdIsSendReadRFID => (_aoVehicleCommand.Value & 0x80) == 0x80;
  448. private bool _currentCmdIsSendClearError => (_aoVehicleCommand.Value & 0x100) == 0x100;
  449. private bool _currentCmdIsSendInit => (_aoVehicleCommand.Value & 0x200) == 0x200;
  450. private uint _currentCmdTargetPosition => BitConverter.ToUInt16(BitConverter.GetBytes(_aoTargetPosition.Value),0);
  451. private float[] _currentCmdViaCoordinates
  452. {
  453. get
  454. {
  455. List<float> values = new List<float>();
  456. for (int i = 0; i < _aoViaCoordinates.Length/2;i++)
  457. {
  458. List<byte> value = new List<byte>();
  459. value.AddRange(BitConverter.GetBytes(_aoViaCoordinates[i * 2].Value));
  460. value.AddRange(BitConverter.GetBytes(_aoViaCoordinates[i * 2 +1].Value));
  461. float coordinatevalue = BitConverter.ToSingle(value.ToArray(), 0);
  462. if (coordinatevalue == 0) break;
  463. values.Add(coordinatevalue);
  464. }
  465. return values.ToArray();
  466. }
  467. }
  468. private string _currentRFID
  469. {
  470. get
  471. {
  472. List<byte> bValues = new List<byte>();
  473. AIAccessor[] aiRfid = {_aiRFIDChar0,_aiRFIDChar1,_aiRFIDChar2,_aiRFIDChar3,_aiRFIDChar4, _aiRFIDChar5, _aiRFIDChar6, _aiRFIDChar7, };
  474. foreach(var ai in aiRfid)
  475. {
  476. bValues.AddRange(BitConverter.GetBytes(ai.Value));
  477. }
  478. string strValue = Encoding.ASCII.GetString(bValues.ToArray());
  479. int index = strValue.IndexOf(null);
  480. if (index != -1)
  481. {
  482. strValue = strValue.Substring(0, index);
  483. }
  484. return strValue;
  485. }
  486. }
  487. private void SubscribeDataVariable()
  488. {
  489. DATA.Subscribe($"{Module}.{Name}.Status", () => ((VehicleStateEnum)fsm.State).ToString());
  490. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateInitializing", () => _stateIsInitilizing);
  491. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateIdle", () => _stateIsIdle);
  492. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateMoving", () => _stateIsMoving);
  493. DATA.Subscribe($"{Module}.{Name}.IsCurrentStatePlacing", () => _stateIsPlacing);
  494. DATA.Subscribe($"{Module}.{Name}.IsCurrentStatePicking", () => _stateIsPicking);
  495. DATA.Subscribe($"{Module}.{Name}.IsCurrentStatePaused", () => _stateIsPaused);
  496. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateWait", () => _stateIsWait);
  497. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateOnline", () => _stateIsOnlineMode);
  498. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateTeachMode", () => _stateIsTeachMode);
  499. DATA.Subscribe($"{Module}.{Name}.IsCurrentStatePicking", () => _stateIsPicking);
  500. DATA.Subscribe($"{Module}.{Name}.IsCurrentStateError", () => _stateIsError);
  501. DATA.Subscribe($"{Module}.{Name}.CurrentTargetPosition", () => _vehicleTargePostion);
  502. DATA.Subscribe($"{Module}.{Name}.CurrentPosition", () => _vehicleCurrentPosition);
  503. DATA.Subscribe($"{Module}.{Name}.CurrentCoordinate", () => _vehicleCurrentCoordinate);
  504. DATA.Subscribe($"{Module}.{Name}.VehicleIpAddress", () => _vehiclePLCIP);
  505. DATA.Subscribe($"{Module}.{Name}.VehicleID", () => _vehicleIdOnPlc);
  506. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyPause", () => _currentCmdIsReplyPause);
  507. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyClearCommand", () => _currentCmdIsReplyClearCommand);
  508. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplySetOnline", () => _currentCmdIsReplySetOnline);
  509. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyMoveToPos", () => _currentCmdIsReplyMoveToPosition);
  510. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyPickFromPos", () => _currentCmdIsReplyPick);
  511. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyPlaceToPos", () => _currentCmdIsReplyPlace);
  512. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyChangeRoute", () => _currentCmdIsReplyChangeRoute);
  513. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyReadRFID", () => _currentCmdIsReplyReadRFID);
  514. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandReplyClearError", () => _currentCmdIsReplyClearError);
  515. DATA.Subscribe($"{Module}.{Name}.CmdResultIsACK", () => _cmdResultIsACK);
  516. DATA.Subscribe($"{Module}.{Name}.CmdResultIsNAK", () => _cmdResultIsNAK);
  517. DATA.Subscribe($"{Module}.{Name}.CmdResultIsComplete", () => _cmdResultIsComplete);
  518. DATA.Subscribe($"{Module}.{Name}.CmdResultIsABS", () => _cmdResultIsAbs);
  519. DATA.Subscribe($"{Module}.{Name}.CmdResultIsRFIDOK", () => _cmdResultIsRfidOK);
  520. DATA.Subscribe($"{Module}.{Name}.CmdResultIsRFIDNG", () => _cmdResultIsRfidNG);
  521. DATA.Subscribe($"{Module}.{Name}.CurrentRFID", () => _currentRFID);
  522. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendPause", () => _currentCmdIsSendPause);
  523. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendClearCommand", () => _currentCmdIsSendClearCommand);
  524. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendSetOnline", () => _currentCmdIsSendSetOnline);
  525. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendMoveToPos", () => _currentCmdIsSendMoveToPosition);
  526. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendPickFromPos", () => _currentCmdIsSendPick);
  527. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendPlaceToPos", () => _currentCmdIsSendPlace);
  528. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendChangeRoute", () => _currentCmdIsSendChangeRoute);
  529. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendReadRFID", () => _currentCmdIsSendReadRFID);
  530. DATA.Subscribe($"{Module}.{Name}.IsCurrentCommandSendClearError", () => _currentCmdIsSendClearError);
  531. }
  532. protected override bool fStartReset(object[] param)
  533. {
  534. CleanCommand();
  535. ResetRoutine();
  536. return base.fStartReset(param);
  537. }
  538. protected override bool fMonitorReset(object[] param)
  539. {
  540. IsBusy = false;
  541. try
  542. {
  543. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoVehicleCommand, (short)0x100, Notify);
  544. WaitCommandComplete((int)VehicleStepEnum.ActionStep2, 90, Notify);
  545. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  546. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  547. WaitSigStateValue((int)VehicleStepEnum.ActionStep5, 10, _stateIsError, false, Notify);
  548. }
  549. catch (RoutineBreakException)
  550. {
  551. return false;
  552. }
  553. catch (RoutineFaildException)
  554. {
  555. CleanCommand();
  556. OnError("ResetTimeout");
  557. return true;
  558. }
  559. return true;
  560. }
  561. protected override bool fStartInit(object[] param)
  562. {
  563. CleanCommand();
  564. ResetRoutine();
  565. return base.fStartInit(param);
  566. }
  567. protected override bool fMonitorInit(object[] param)
  568. {
  569. IsBusy = false;
  570. try
  571. {
  572. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoVehicleCommand, (short)0x200, Notify);
  573. WaitCommandComplete((int)VehicleStepEnum.ActionStep2, 90,Notify);
  574. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  575. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  576. }
  577. catch (RoutineBreakException)
  578. {
  579. return false;
  580. }
  581. catch (RoutineFaildException)
  582. {
  583. CleanCommand();
  584. OnError("ResetTimeout");
  585. return true;
  586. }
  587. return true;
  588. }
  589. protected override bool fStartPause(object[] param)
  590. {
  591. CleanCommand();
  592. ResetRoutine();
  593. return base.fStartInit(param);
  594. }
  595. protected override bool fMonitorPausing(object[] param)
  596. {
  597. IsBusy = false;
  598. try
  599. {
  600. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoVehicleCommand, (short)0x1, Notify);
  601. WaitCommandComplete((int)VehicleStepEnum.ActionStep2, 90, Notify);
  602. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  603. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  604. WaitSigStateValue((int)VehicleStepEnum.ActionStep4, 60, _stateIsPaused, true, Notify);
  605. }
  606. catch (RoutineBreakException)
  607. {
  608. return false;
  609. }
  610. catch (RoutineFaildException)
  611. {
  612. CleanCommand();
  613. OnError("ResetTimeout");
  614. return true;
  615. }
  616. return true;
  617. }
  618. protected override bool fStartMove(object[] param)
  619. {
  620. CleanCommand();
  621. ResetRoutine();
  622. return base.fStartMove(param);
  623. }
  624. protected override bool fMonitorMoving(object[] param)
  625. {
  626. IsBusy = false;
  627. try
  628. {
  629. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoTargetPosition, (short)CurrentParameters[0], Notify);
  630. SetViaPositionValue((int)VehicleStepEnum.ActionStep2, (ushort[])CurrentParameters[1], Notify);
  631. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x8, Notify);
  632. WaitCommandComplete((int)VehicleStepEnum.ActionStep4, 12000, Notify);
  633. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  634. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  635. WaitSigStateValue((int)VehicleStepEnum.ActionStep4, 60, _stateIsPaused, true, Notify);
  636. }
  637. catch (RoutineBreakException)
  638. {
  639. return false;
  640. }
  641. catch (RoutineFaildException)
  642. {
  643. CleanCommand();
  644. OnError("ResetTimeout");
  645. return true;
  646. }
  647. return true;
  648. }
  649. protected override bool fStartPick(object[] param)
  650. {
  651. CleanCommand();
  652. ResetRoutine();
  653. return base.fStartPick(param);
  654. }
  655. protected override bool fMonitorPicking(object[] param)
  656. {
  657. IsBusy = false;
  658. try
  659. {
  660. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoTargetPosition, (short)CurrentParameters[0], Notify);
  661. SetViaPositionValue((int)VehicleStepEnum.ActionStep2, (ushort[])CurrentParameters[1], Notify);
  662. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x10, Notify);
  663. WaitCommandComplete((int)VehicleStepEnum.ActionStep4, 12000, Notify);
  664. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  665. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  666. WaitSigStateValue((int)VehicleStepEnum.ActionStep4, 60, _stateIsPaused, true, Notify);
  667. }
  668. catch (RoutineBreakException)
  669. {
  670. return false;
  671. }
  672. catch (RoutineFaildException)
  673. {
  674. CleanCommand();
  675. OnError("ResetTimeout");
  676. return true;
  677. }
  678. return true;
  679. }
  680. protected override bool fStartPlace(object[] param)
  681. {
  682. CleanCommand();
  683. ResetRoutine();
  684. return base.fStartPlace(param);
  685. }
  686. protected override bool fMonitorPlacing(object[] param)
  687. {
  688. IsBusy = false;
  689. try
  690. {
  691. SetAoValue((int)VehicleStepEnum.ActionStep1, _aoTargetPosition, (short)CurrentParameters[0], Notify);
  692. SetViaPositionValue((int)VehicleStepEnum.ActionStep2, (ushort[])CurrentParameters[1], Notify);
  693. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x20, Notify);
  694. WaitCommandComplete((int)VehicleStepEnum.ActionStep4, 12000, Notify);
  695. SetAoValue((int)VehicleStepEnum.ActionStep3, _aoVehicleCommand, (short)0x000, Notify);
  696. WaitCommandStatusClear((int)VehicleStepEnum.ActionStep4, 10, Notify);
  697. WaitSigStateValue((int)VehicleStepEnum.ActionStep4, 60, _stateIsPaused, true, Notify);
  698. }
  699. catch (RoutineBreakException)
  700. {
  701. return false;
  702. }
  703. catch (RoutineFaildException)
  704. {
  705. CleanCommand();
  706. OnError("ResetTimeout");
  707. return true;
  708. }
  709. return true;
  710. }
  711. private void CleanCommand()
  712. {
  713. _aoVehicleCommand.Value = 0;
  714. }
  715. public AOAccessor ParseAoNode(string name, XmlElement node, string ioModule = "")
  716. {
  717. if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim()))
  718. return IO.AO[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim() + $"_{Name}" : $"{ioModule}.{node.GetAttribute(name).Trim()}_{Name}"];
  719. return null;
  720. }
  721. public AIAccessor ParseAiNode(string name, XmlElement node, string ioModule = "")
  722. {
  723. if (!string.IsNullOrEmpty(node.GetAttribute(name).Trim()))
  724. return IO.AI[string.IsNullOrEmpty(ioModule) ? node.GetAttribute(name).Trim()+$"_{Name}" : $"{ioModule}.{node.GetAttribute(name).Trim()}_{Name}"];
  725. return null;
  726. }
  727. private void SetViaPositionValue(int id, ushort[] positions, Action<string> notify)
  728. {
  729. var ret = Execute(id, () =>
  730. {
  731. ClearViaData();
  732. int count = positions.Length >= 50 ? 50 : positions.Length;
  733. for (int i=0;i< count; i++)
  734. {
  735. SetViaCoordinateValue(positions[i], _aoViaCoordinates[2 * i], _aoViaCoordinates[2 * i + 1]);
  736. }
  737. return true;
  738. });
  739. if (ret.Item1)
  740. {
  741. if (ret.Item2 == Result.FAIL)
  742. {
  743. throw new RoutineFaildException();
  744. }
  745. }
  746. }
  747. private void SetViaCoordinateValue(ushort postion,AOAccessor lowbits,AOAccessor highbits)
  748. {
  749. float coordinate = GetCoordinateValue(postion);
  750. byte[] bits = BitConverter.GetBytes(coordinate);
  751. lowbits.Value = BitConverter.ToInt16(bits, 0);
  752. highbits.Value = BitConverter.ToInt16(bits, 2);
  753. //lowbits.Value = bits.
  754. }
  755. private float GetCoordinateValue(ushort position)
  756. {
  757. return 6553.55F;
  758. }
  759. private void ClearViaData()
  760. {
  761. for(int i=1;i<=50; i++)
  762. {
  763. }
  764. }
  765. private void SetAoValue(int id, AOAccessor ao, short value, Action<string> notify)
  766. {
  767. var ret = Execute(id, () =>
  768. {
  769. notify($"{Name} set {ao.Name} to {value}.");
  770. ao.Value = value;
  771. return true;
  772. });
  773. if (ret.Item1)
  774. {
  775. if (ret.Item2 == Result.FAIL)
  776. {
  777. throw new RoutineFaildException();
  778. }
  779. }
  780. }
  781. private void WaitCommandComplete(int id, int time, Action<string> notify)
  782. {
  783. var ret = ExecuteAndWait(id, () =>
  784. {
  785. notify($"Wait Vehicle command ACK State to be true.");
  786. return true;
  787. }, () =>
  788. {
  789. if (_cmdResultIsACK && _cmdResultIsComplete && !_cmdResultIsAbs && !_cmdResultIsNAK)
  790. return true;
  791. if(_cmdResultIsNAK || _cmdResultIsAbs)
  792. throw new RoutineFaildException();
  793. return false;
  794. }, time * 1000);
  795. if (ret.Item1)
  796. {
  797. if (ret.Item2 == Result.FAIL)
  798. {
  799. throw new RoutineFaildException();
  800. }
  801. else if (ret.Item2 == Result.TIMEOUT) //timeout
  802. {
  803. OnError($"Wait Vehicle command execution complete timeout after {time} seconds");
  804. throw new RoutineFaildException();
  805. }
  806. else
  807. {
  808. throw new RoutineBreakException();
  809. }
  810. }
  811. }
  812. private void WaitCommandStatusClear(int id, int time, Action<string> notify)
  813. {
  814. var ret = ExecuteAndWait(id, () =>
  815. {
  816. notify($"Wait Vehicle command State to be clear.");
  817. return true;
  818. }, () =>
  819. {
  820. if (!_cmdResultIsACK && !_cmdResultIsComplete && !_cmdResultIsAbs && !_cmdResultIsNAK)
  821. return true;
  822. return false;
  823. }, time * 1000);
  824. if (ret.Item1)
  825. {
  826. if (ret.Item2 == Result.FAIL)
  827. {
  828. throw new RoutineFaildException();
  829. }
  830. else if (ret.Item2 == Result.TIMEOUT) //timeout
  831. {
  832. OnError($"Wait Vehicle command status clear timeout after {time} seconds");
  833. throw new RoutineFaildException();
  834. }
  835. else
  836. {
  837. throw new RoutineBreakException();
  838. }
  839. }
  840. }
  841. private void WaitSigStateValue(int id, int time, bool signalState, bool value, Action<string> notify)
  842. {
  843. var ret = ExecuteAndWait(id, () =>
  844. {
  845. notify($"Wait Vehicle {nameof(signalState)} State to be {value}");
  846. return true;
  847. }, () =>
  848. {
  849. if (signalState == value)
  850. return true;
  851. return false;
  852. }, time * 1000);
  853. if (ret.Item1)
  854. {
  855. if (ret.Item2 == Result.FAIL)
  856. {
  857. throw new RoutineFaildException();
  858. }
  859. else if (ret.Item2 == Result.TIMEOUT) //timeout
  860. {
  861. OnError($"Wait Vehicle {Name} to be {value} timeout after {time} seconds");
  862. throw new RoutineFaildException();
  863. }
  864. else
  865. {
  866. throw new RoutineBreakException();
  867. }
  868. }
  869. }
  870. private void WaitAiValue(int id, int time, AIAccessor ai, short value, Action<string> notify, Action<string> error)
  871. {
  872. var ret = ExecuteAndWait(id, () =>
  873. {
  874. notify($"Wait Vehicle {Name} {ai.Name} to be {value}");
  875. return true;
  876. }, () =>
  877. {
  878. if (ai.Value == value)
  879. return true;
  880. return false;
  881. }, time * 1000);
  882. if (ret.Item1)
  883. {
  884. if (ret.Item2 == Result.FAIL)
  885. {
  886. throw new RoutineFaildException();
  887. }
  888. else if (ret.Item2 == Result.TIMEOUT) //timeout
  889. {
  890. OnError($"Wait Vehicle {Name} {ai.Name} to be {value} timeout after {time} seconds");
  891. throw new RoutineFaildException();
  892. }
  893. else
  894. {
  895. throw new RoutineBreakException();
  896. }
  897. }
  898. }
  899. protected void Notify(string message)
  900. {
  901. EV.PostMessage(Name, EventEnum.GeneralInfo, string.Format("{0}:{1}", Name, message));
  902. }
  903. protected void Stop(string failReason)
  904. {
  905. OnError(string.Format("Failed {0}, {1} ", Name, failReason));
  906. }
  907. private enum VehicleStepEnum
  908. {
  909. ActionStep1,
  910. ActionStep2,
  911. ActionStep3,
  912. ActionStep4,
  913. ActionStep5,
  914. ActionStep6,
  915. ActionStep7,
  916. ActionStep8,
  917. ActionStep9,
  918. ActionStep10,
  919. ActionStep11,
  920. ActionStep12,
  921. ActionStep13,
  922. ActionStep14,
  923. ActionStep15,
  924. ActionStep16,
  925. }
  926. //timer, 计算routine时间
  927. protected DeviceTimer counter = new DeviceTimer();
  928. protected DeviceTimer delayTimer = new DeviceTimer();
  929. private enum STATE
  930. {
  931. IDLE,
  932. WAIT,
  933. }
  934. public int TokenId
  935. {
  936. get { return _id; }
  937. }
  938. private int _id; //step index
  939. /// <summary>
  940. /// already done steps
  941. /// </summary>
  942. private Stack<int> _steps = new Stack<int>();
  943. private STATE state; //step state //idel,wait,
  944. //loop control
  945. private int loop = 0;
  946. private int loopCount = 0;
  947. private int loopID = 0;
  948. private DeviceTimer timer = new DeviceTimer();
  949. public int LoopCounter { get { return loop; } }
  950. public int LoopTotalTime { get { return loopCount; } }
  951. // public int Timeout { get { return (int)(timer.GetTotalTime() / 1000); } }
  952. //状态持续时间,单位为秒
  953. public int Elapsed { get { return (int)(timer.GetElapseTime() / 1000); } }
  954. protected RoutineResult RoutineToken = new RoutineResult() { Result = RoutineState.Running };
  955. public void ResetRoutine()
  956. {
  957. _id = 0;
  958. _steps.Clear();
  959. loop = 0;
  960. loopCount = 0;
  961. state = STATE.IDLE;
  962. counter.Start(60 * 60 * 100); //默认1小时
  963. RoutineToken.Result = RoutineState.Running;
  964. }
  965. protected void PerformRoutineStep(int id, Func<RoutineState> execution, RoutineResult result)
  966. {
  967. if (!Acitve(id))
  968. return;
  969. result.Result = execution();
  970. }
  971. #region interface
  972. public void StopLoop()
  973. {
  974. loop = loopCount;
  975. }
  976. public Tuple<bool, Result> Loop<T>(T id, Func<bool> func, int count)
  977. {
  978. int idx = Convert.ToInt32(id);
  979. bool bActive = Acitve(idx);
  980. if (bActive)
  981. {
  982. if (!func())
  983. {
  984. return Tuple.Create(bActive, Result.FAIL); //执行错误
  985. }
  986. loopID = idx;
  987. loopCount = count;
  988. next();
  989. return Tuple.Create(true, Result.RUN);
  990. }
  991. return Tuple.Create(false, Result.RUN);
  992. }
  993. public Tuple<bool, Result> EndLoop<T>(T id, Func<bool> func)
  994. {
  995. int idx = Convert.ToInt32(id);
  996. bool bActive = Acitve(idx);
  997. if (bActive)
  998. {
  999. if (++loop >= loopCount) //Loop 结束
  1000. {
  1001. if (!func())
  1002. {
  1003. return Tuple.Create(bActive, Result.FAIL); //执行错误
  1004. }
  1005. loop = 0;
  1006. loopCount = 0; // Loop 结束时,当前loop和loop总数都清零
  1007. next();
  1008. return Tuple.Create(true, Result.RUN);
  1009. }
  1010. //继续下一LOOP
  1011. next(loopID);
  1012. return Tuple.Create(true, Result.RUN);
  1013. }
  1014. return Tuple.Create(false, Result.RUN);
  1015. }
  1016. public Tuple<bool, Result> ExecuteAndWait<T>(T id, IRoutine routine)
  1017. {
  1018. int idx = Convert.ToInt32(id);
  1019. bool bActive = Acitve(idx);
  1020. if (bActive)
  1021. {
  1022. if (state == STATE.IDLE)
  1023. {
  1024. Result startRet = routine.Start();
  1025. if (startRet == Result.FAIL)
  1026. {
  1027. return Tuple.Create(true, Result.FAIL); //执行错误
  1028. }
  1029. else if (startRet == Result.DONE)
  1030. {
  1031. next();
  1032. return Tuple.Create(true, Result.DONE);
  1033. }
  1034. state = STATE.WAIT;
  1035. }
  1036. Result ret = routine.Monitor();
  1037. if (ret == Result.DONE)
  1038. {
  1039. next();
  1040. return Tuple.Create(true, Result.DONE);
  1041. }
  1042. else if (ret == Result.FAIL || ret == Result.TIMEOUT)
  1043. {
  1044. return Tuple.Create(true, Result.FAIL);
  1045. }
  1046. else
  1047. {
  1048. return Tuple.Create(true, Result.RUN);
  1049. }
  1050. }
  1051. return Tuple.Create(false, Result.RUN);
  1052. }
  1053. public Tuple<bool, Result> ExecuteAndWait<T>(T id, List<IRoutine> routines)
  1054. {
  1055. int idx = Convert.ToInt32(id);
  1056. bool bActive = Acitve(idx);
  1057. if (bActive)
  1058. {
  1059. if (state == STATE.IDLE)
  1060. {
  1061. foreach (var item in routines)
  1062. {
  1063. if (item.Start() == Result.FAIL)
  1064. return Tuple.Create(true, Result.FAIL);
  1065. }
  1066. state = STATE.WAIT;
  1067. }
  1068. //wait all sub failed or completedboo
  1069. bool bFail = false;
  1070. bool bDone = true;
  1071. foreach (var item in routines)
  1072. {
  1073. Result ret = item.Monitor();
  1074. bDone &= (ret == Result.FAIL || ret == Result.DONE);
  1075. bFail |= ret == Result.FAIL;
  1076. }
  1077. if (bDone)
  1078. {
  1079. next();
  1080. if (bFail)
  1081. return Tuple.Create(true, Result.FAIL);
  1082. return Tuple.Create(true, Result.DONE);
  1083. }
  1084. return Tuple.Create(true, Result.RUN);
  1085. }
  1086. return Tuple.Create(false, Result.RUN);
  1087. }
  1088. public Tuple<bool, Result> Check<T>(T id, Func<bool> func) //顺序执行
  1089. {
  1090. return Check(Check(Convert.ToInt32(id), func));
  1091. }
  1092. public Tuple<bool, Result> Execute<T>(T id, Func<bool> func) //顺序执行
  1093. {
  1094. return Check(execute(Convert.ToInt32(id), func));
  1095. }
  1096. public Tuple<bool, Result> Wait<T>(T id, Func<bool> func, double timeout = int.MaxValue) //Wait condition
  1097. {
  1098. return Check(wait(Convert.ToInt32(id), func, timeout));
  1099. }
  1100. public Tuple<bool, Result> Wait<T>(T id, Func<bool?> func, double timeout = int.MaxValue) //Wait condition
  1101. {
  1102. return Check(wait(Convert.ToInt32(id), func, timeout));
  1103. }
  1104. public Tuple<bool, Result> ExecuteAndWait<T>(T id, Func<bool> execute, Func<bool?> check, double timeout = int.MaxValue)
  1105. {
  1106. int idx = Convert.ToInt32(id);
  1107. bool bActive = Acitve(idx);
  1108. bool? bExecute = false;
  1109. if (bActive)
  1110. {
  1111. if (state == STATE.IDLE)
  1112. {
  1113. if (!execute())
  1114. {
  1115. return Tuple.Create(bActive, Result.FAIL); //执行错误
  1116. }
  1117. timer.Start(timeout);
  1118. state = STATE.WAIT;
  1119. }
  1120. bExecute = check();
  1121. if (bExecute == null)
  1122. {
  1123. return Tuple.Create(bActive, Result.FAIL); //Termianate
  1124. }
  1125. else
  1126. {
  1127. if (bExecute.Value) //检查Success, next
  1128. {
  1129. next();
  1130. return Tuple.Create(true, Result.RUN);
  1131. }
  1132. }
  1133. if (timer.IsTimeout())
  1134. return Tuple.Create(true, Result.TIMEOUT);
  1135. return Tuple.Create(true, Result.RUN);
  1136. }
  1137. return Tuple.Create(false, Result.RUN);
  1138. }
  1139. public Tuple<bool, Result> ExecuteAndWait<T>(T id, Func<bool> execute, Func<bool?> check, Func<double> time)
  1140. {
  1141. int idx = Convert.ToInt32(id);
  1142. bool bActive = Acitve(idx);
  1143. bool? bExecute = false;
  1144. double timeout = 0;
  1145. if (bActive)
  1146. {
  1147. if (state == STATE.IDLE)
  1148. {
  1149. timeout = time();
  1150. if (!execute())
  1151. {
  1152. return Tuple.Create(true, Result.FAIL); //执行错误
  1153. }
  1154. timer.Start(timeout);
  1155. state = STATE.WAIT;
  1156. }
  1157. bExecute = check();
  1158. if (bExecute == null)
  1159. {
  1160. return Tuple.Create(true, Result.FAIL); //Termianate
  1161. }
  1162. if (bExecute.Value) //检查Success, next
  1163. {
  1164. next();
  1165. return Tuple.Create(true, Result.RUN);
  1166. }
  1167. if (timer.IsTimeout())
  1168. return Tuple.Create(true, Result.TIMEOUT);
  1169. return Tuple.Create(true, Result.RUN);
  1170. }
  1171. return Tuple.Create(false, Result.RUN);
  1172. }
  1173. public Tuple<bool, Result> Wait<T>(T id, IRoutine rt)
  1174. {
  1175. int idx = Convert.ToInt32(id);
  1176. bool bActive = Acitve(idx);
  1177. if (bActive)
  1178. {
  1179. if (state == STATE.IDLE)
  1180. {
  1181. rt.Start();
  1182. state = STATE.WAIT;
  1183. }
  1184. Result ret = rt.Monitor();
  1185. return Tuple.Create(true, ret);
  1186. }
  1187. return Tuple.Create(false, Result.RUN);
  1188. }
  1189. //Monitor
  1190. public Tuple<bool, Result> Monitor<T>(T id, Func<bool> func, Func<bool> check, double time)
  1191. {
  1192. int idx = Convert.ToInt32(id);
  1193. bool bActive = Acitve(idx);
  1194. bool bCheck = false;
  1195. if (bActive)
  1196. {
  1197. if (state == STATE.IDLE)
  1198. {
  1199. if ((func != null) && !func())
  1200. {
  1201. return Tuple.Create(true, Result.FAIL);
  1202. }
  1203. timer.Start(time);
  1204. state = STATE.WAIT;
  1205. }
  1206. bCheck = check();
  1207. if (!bCheck)
  1208. {
  1209. return Tuple.Create(true, Result.FAIL); //Termianate
  1210. }
  1211. if (timer.IsTimeout())
  1212. {
  1213. next();
  1214. }
  1215. return Tuple.Create(true, Result.RUN);
  1216. }
  1217. return Tuple.Create(false, Result.RUN);
  1218. }
  1219. //Delay
  1220. public Tuple<bool, Result> Delay<T>(T id, Func<bool> func, double time)
  1221. {
  1222. int idx = Convert.ToInt32(id);
  1223. bool bActive = Acitve(idx);
  1224. if (bActive)
  1225. {
  1226. if (state == STATE.IDLE)
  1227. {
  1228. if ((func != null) && !func())
  1229. {
  1230. return Tuple.Create(true, Result.FAIL);
  1231. }
  1232. timer.Start(time);
  1233. state = STATE.WAIT;
  1234. }
  1235. if (timer.IsTimeout())
  1236. {
  1237. next();
  1238. }
  1239. return Tuple.Create(true, Result.RUN);
  1240. }
  1241. return Tuple.Create(false, Result.RUN);
  1242. }
  1243. //先delay 再运行
  1244. public Tuple<bool, Result> DelayCheck<T>(T id, Func<bool> func, double time)
  1245. {
  1246. int idx = Convert.ToInt32(id);
  1247. bool bActive = Acitve(idx);
  1248. if (bActive)
  1249. {
  1250. if (state == STATE.IDLE)
  1251. {
  1252. timer.Start(time);
  1253. state = STATE.WAIT;
  1254. }
  1255. if (timer.IsTimeout())
  1256. {
  1257. if (func != null && !func())
  1258. {
  1259. return Tuple.Create(true, Result.FAIL);
  1260. }
  1261. next();
  1262. }
  1263. return Tuple.Create(true, Result.RUN);
  1264. }
  1265. return Tuple.Create(false, Result.RUN);
  1266. }
  1267. #endregion
  1268. private Tuple<bool, bool> execute(int id, Func<bool> func) //顺序执行
  1269. {
  1270. bool bActive = Acitve(id);
  1271. bool bExecute = false;
  1272. if (bActive)
  1273. {
  1274. bExecute = func();
  1275. if (bExecute)
  1276. {
  1277. next();
  1278. }
  1279. }
  1280. return Tuple.Create(bActive, bExecute);
  1281. }
  1282. private Tuple<bool, bool> Check(int id, Func<bool> func) //check
  1283. {
  1284. bool bActive = Acitve(id);
  1285. bool bExecute = false;
  1286. if (bActive)
  1287. {
  1288. bExecute = func();
  1289. next();
  1290. }
  1291. return Tuple.Create(bActive, bExecute);
  1292. }
  1293. /// <summary>
  1294. /// </summary>
  1295. /// <param name="id"></param>
  1296. /// <param name="func"></param>
  1297. /// <param name="timeout"></param>
  1298. /// <returns>
  1299. /// item1 Active
  1300. /// item2 execute
  1301. /// item3 Timeout
  1302. ///</returns>
  1303. private Tuple<bool, bool, bool> wait(int id, Func<bool> func, double timeout = int.MaxValue) //Wait condition
  1304. {
  1305. bool bActive = Acitve(id);
  1306. bool bExecute = false;
  1307. bool bTimeout = false;
  1308. if (bActive)
  1309. {
  1310. if (state == STATE.IDLE)
  1311. {
  1312. timer.Start(timeout);
  1313. state = STATE.WAIT;
  1314. }
  1315. bExecute = func();
  1316. if (bExecute)
  1317. {
  1318. next();
  1319. }
  1320. bTimeout = timer.IsTimeout();
  1321. }
  1322. return Tuple.Create(bActive, bExecute, bTimeout);
  1323. }
  1324. private Tuple<bool, bool?, bool> wait(int id, Func<bool?> func, double timeout = int.MaxValue) //Wait condition && Check error
  1325. {
  1326. bool bActive = Acitve(id);
  1327. bool? bExecute = false;
  1328. bool bTimeout = false;
  1329. if (bActive)
  1330. {
  1331. if (state == STATE.IDLE)
  1332. {
  1333. timer.Start(timeout);
  1334. state = STATE.WAIT;
  1335. }
  1336. bExecute = func();
  1337. if (bExecute.HasValue && bExecute.Value)
  1338. {
  1339. next();
  1340. }
  1341. bTimeout = timer.IsTimeout();
  1342. }
  1343. return Tuple.Create(bActive, bExecute, bTimeout);
  1344. }
  1345. /// <summary>
  1346. /// </summary>
  1347. /// <param name="value"></param>
  1348. /// <returns>
  1349. /// item1 true, return item2
  1350. /// </returns>
  1351. private Tuple<bool, Result> Check(Tuple<bool, bool> value)
  1352. {
  1353. if (value.Item1)
  1354. {
  1355. if (!value.Item2)
  1356. {
  1357. return Tuple.Create(true, Result.FAIL);
  1358. }
  1359. return Tuple.Create(true, Result.RUN);
  1360. }
  1361. return Tuple.Create(false, Result.RUN);
  1362. }
  1363. private Tuple<bool, Result> Check(Tuple<bool, bool, bool> value)
  1364. {
  1365. if (value.Item1) // 当前执行
  1366. {
  1367. if (CheckTimeout(value)) //timeout
  1368. {
  1369. return Tuple.Create(true, Result.TIMEOUT);
  1370. }
  1371. return Tuple.Create(true, Result.RUN);
  1372. }
  1373. return Tuple.Create(false, Result.RUN);
  1374. }
  1375. private Tuple<bool, Result> Check(Tuple<bool, bool?, bool> value)
  1376. {
  1377. if (value.Item1) // 当前执行
  1378. {
  1379. if (value.Item2 == null)
  1380. {
  1381. return Tuple.Create(true, Result.FAIL);
  1382. }
  1383. else
  1384. {
  1385. if (value.Item2 == false && value.Item3 == true) //timeout
  1386. {
  1387. return Tuple.Create(true, Result.TIMEOUT);
  1388. }
  1389. return Tuple.Create(true, Result.RUN);
  1390. }
  1391. }
  1392. return Tuple.Create(false, Result.RUN);
  1393. }
  1394. private bool CheckTimeout(Tuple<bool, bool, bool> value)
  1395. {
  1396. return value.Item1 == true && value.Item2 == false && value.Item3 == true;
  1397. }
  1398. private bool Acitve(int id) //
  1399. {
  1400. if (_steps.Contains(id))
  1401. return false;
  1402. this._id = id;
  1403. return true;
  1404. }
  1405. private void next()
  1406. {
  1407. _steps.Push(this._id);
  1408. state = STATE.IDLE;
  1409. }
  1410. private void next(int step) //loop
  1411. {
  1412. while (_steps.Pop() != step) ;
  1413. state = STATE.IDLE;
  1414. }
  1415. public void Delay(int id, double delaySeconds)
  1416. {
  1417. Tuple<bool, Result> ret = Delay(id, () =>
  1418. {
  1419. return true;
  1420. }, delaySeconds * 1000);
  1421. if (ret.Item1)
  1422. {
  1423. if (ret.Item2 == Result.RUN)
  1424. {
  1425. throw (new RoutineBreakException());
  1426. }
  1427. }
  1428. }
  1429. public bool IsActived(int id)
  1430. {
  1431. return _steps.Contains(id);
  1432. }
  1433. }
  1434. }