YaskawaSR100RobotSimulator.cs 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368
  1. using System;
  2. using System.Collections.Generic;
  3. using System.Linq;
  4. using System.Text;
  5. using System.Threading;
  6. using System.Threading.Tasks;
  7. using MECF.Framework.Simulator.Core.Driver;
  8. namespace MECF.Framework.Simulator.Core.Robots
  9. {
  10. public class YaskawaSR100RobotSimulator : YaskawaSR100ControllerSimulator
  11. {
  12. public YaskawaSR100RobotSimulator()
  13. : base(10110)
  14. {
  15. }
  16. //$,<UNo>(,<SeqNo>),INIT,<ErrClr>,<SrvOn>,<Home>(,<Sum>)<CR>
  17. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,INIT,<ErrClr>,<SrvOn>,<Axis>(,<Sum>)<CR>
  18. //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,INIT,<ExeTime>,<PosData1>…,<PosDataN>(,<Sum>)<CR>
  19. public override void HandleINIT(string msg)
  20. {
  21. string[] arrayMsg = msg.Split(',');
  22. string unit = arrayMsg[1];
  23. string seq = arrayMsg[2];
  24. string cmd = arrayMsg[3];
  25. string sts = 0.ToString("X02");
  26. string Ackcd = 0.ToString("X04");
  27. string errorClr = arrayMsg[4];
  28. string srvOn = arrayMsg[5];
  29. string axis = arrayMsg[6];
  30. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, Ackcd, cmd, errorClr, srvOn, axis);
  31. string sum = CheckSum(feedback);
  32. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  33. OnWriteMessage(result);
  34. Thread.Sleep(3000);
  35. Random rd = new Random();
  36. int executeTime = rd.Next(10000, 999999);
  37. string exeTime = executeTime.ToString("D6");
  38. string posData = GetPosData();
  39. string error = 0.ToString("X04");
  40. feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, cmd, exeTime, posData);
  41. sum = CheckSum(feedback);
  42. result = String.Format("{0}{1}{2}", "!", feedback, sum);
  43. OnWriteMessage(result);
  44. }
  45. //$,<UNo>(,<SeqNo>),CCLR,<CMode>(,<Sum>)<CR>
  46. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,CCLR,<CMode>(,<Sum>)<CR>
  47. //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,CCLR,<ExeTime>,<PosData1>…,<PosDataN>(,<Sum>)<CR>
  48. public override void HandleCCLR(string msg)
  49. {
  50. string[] arrayMsg = msg.Split(',');
  51. string unit = arrayMsg[1];
  52. string seq = arrayMsg[2];
  53. string cmd = arrayMsg[3];
  54. string sts = 0.ToString("X02");
  55. string Ackcd = 0.ToString("X04");
  56. string cmode = arrayMsg[4];
  57. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},", unit, seq, sts, Ackcd, cmd, cmode);
  58. string sum = CheckSum(feedback);
  59. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  60. OnWriteMessage(result);
  61. Thread.Sleep(3000);
  62. int executeTime = _rd.Next(10000, 999999);
  63. string exeTime = executeTime.ToString("D6");
  64. string posData = GetPosData();
  65. string error = 0.ToString("X04");
  66. feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, cmd, exeTime, posData);
  67. sum = CheckSum(feedback);
  68. result = String.Format("{0}{1}{2}", "!", feedback, sum);
  69. OnWriteMessage(result);
  70. }
  71. //$,<UNo>(,<SeqNo>),RSTS(,<Sum>)<CR>
  72. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,RSTS,<Errcd>,<Status>(,<Sum>)<CR>
  73. public override void HandleRSTS(string msg)
  74. {
  75. string[] arrayMsg = msg.Split(',');
  76. string unit = arrayMsg[1];
  77. string seq = arrayMsg[2];
  78. string cmd = arrayMsg[3];
  79. string sts = 0.ToString("X02");
  80. string Ackcd = 0.ToString("X04");
  81. string error = 0.ToString("X04");
  82. int status1 = Convert.ToInt32("0011", 2);
  83. string status = string.Format("{0}000", status1.ToString("X1"));
  84. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, error, status);
  85. string sum = CheckSum(feedback);
  86. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  87. OnWriteMessage(result);
  88. }
  89. //$,<UNo>(,<SeqNo>),MALN,<Mode>,<Angle>(,<Sum>)<CR>
  90. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,MALN,<Mode>,<Angle>(,<Sum>)<CR>
  91. //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,MALN,<ExeTime>,<PosData1>…,<PosDataN>,<Value1>…,<Value10>(,<Sum>)<CR>
  92. public override void HandleMALN(string msg)
  93. {
  94. string[] arrayMsg = msg.Split(',');
  95. string unit = arrayMsg[1];
  96. string seq = arrayMsg[2];
  97. string cmd = arrayMsg[3];
  98. string sts = 0.ToString("X02");
  99. string Ackcd = 0.ToString("X04");
  100. string mode = arrayMsg[4];
  101. string angle = arrayMsg[5];
  102. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, mode, angle);
  103. string sum = CheckSum(feedback);
  104. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  105. OnWriteMessage(result);
  106. Thread.Sleep(3000);
  107. string error = 0.ToString("X04");
  108. int executeTime = _rd.Next(10000, 999999);
  109. string exeTime = executeTime.ToString("D6");
  110. string posData = GetPosData();
  111. string valueData = GetValueData();
  112. feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, error, cmd, exeTime, posData, valueData);
  113. sum = CheckSum(feedback);
  114. result = String.Format("{0}{1}{2}", "!", feedback, sum);
  115. OnWriteMessage(result);
  116. }
  117. //$,<UNo>(,<SeqNo>),MTRS,<Mtn>,<TrsSt>,<Slot>,<Posture>,<Hand>,<TrsPnt>(,<OfstX>,<OfstY>,<OfstZ>)(,<Angle>)(,<Sum>)<CR>
  118. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,MTRS,<Mtn>,<TrsSt>,<Slot>,<Posture>,<Hand>,<TrsPnt>(,<OfstX>,<OfstY>,<OfstZ>)(,<Angle>)(,<Sum>)<CR>
  119. //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,MTRS,<ExeTime>,<PosData1>…,<PosDataN>(,<Sum>)<CR>
  120. public override void HandleMTRS(string msg)
  121. {
  122. string[] arrayMsg = msg.Split(',');
  123. string unit = arrayMsg[1];
  124. string seq = arrayMsg[2];
  125. string cmd = arrayMsg[3];
  126. string sts = 0.ToString("X02");
  127. string Ackcd = 0.ToString("X04");
  128. string leftAll = "";
  129. for (int i = 4; i < arrayMsg.Length - 1; i++)
  130. leftAll += arrayMsg[i] + ",";
  131. string feedback = string.Format(",{0},{1},{2},{3},{4},{5}", unit, seq, sts, Ackcd, cmd, leftAll);
  132. string sum = CheckSum(feedback);
  133. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  134. OnWriteMessage(result);
  135. Thread.Sleep(3000);
  136. string error = 0.ToString("X04");
  137. int executeTime = _rd.Next(10000, 999999);
  138. string exeTime = executeTime.ToString("D6");
  139. string posData = GetPosData();
  140. feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, cmd, exeTime, posData);
  141. sum = CheckSum(feedback);
  142. result = String.Format("{0}{1}{2}", "!", feedback, sum);
  143. OnWriteMessage(result);
  144. OnWriteMessage(result);
  145. }
  146. //$,<UNo>(,<SeqNo>),RMAP,<TrsSt>,<Slot>(,<Sum>)<CR>
  147. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,RMAP,<TrsSt>,<Slot>,01:<Result1>…,N:<ResultN>(,<Sum>)<CR>
  148. public override void HandleRMAP(string msg)
  149. {
  150. Random rd = new Random();
  151. string[] arrayMsg = msg.Split(',');
  152. string unit = arrayMsg[1];
  153. string seq = arrayMsg[2];
  154. string cmd = arrayMsg[3];
  155. string trs = arrayMsg[4];
  156. string slot = arrayMsg[5];
  157. string sts = 0.ToString("X02");
  158. string Ackcd = 0.ToString("X04");
  159. string error = 0.ToString("X04");
  160. //bool random = false;
  161. bool full = trs == "C01" || trs == "C02";
  162. //string mapData = GetMapData(random, full);
  163. bool pre = trs == "C01" || trs == "C02";
  164. bool mid = trs == "C03" || trs == "C04";
  165. bool post = trs == "C05" || trs == "C06";
  166. string mapData = full? GetMapData(true, true, true) : GetMapData(pre, mid, post);
  167. if (trs == "C01")
  168. mapData = GetMapData(1, 2);
  169. if (trs == "C02")
  170. mapData = GetMapData(4, 5);
  171. if (trs == "C07")
  172. mapData = GetMapData(4, 5);
  173. if (trs == "C08")
  174. mapData = GetMapData(4, 5);
  175. if (trs == "C06")
  176. mapData = GetMapData(4, 5);
  177. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, Ackcd, cmd, trs, slot, mapData);
  178. string sum = CheckSum(feedback);
  179. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  180. OnWriteMessage(result);
  181. }
  182. //$,<UNo>(,<SeqNo>),MMAP,<TrsSt>,<Slot>(,<Safe>)(,<Sum>)<CR>
  183. //$,<UNo>(,<SeqNo>),<Sts>,<Ackcd>,MMAP,<TrsSt>,<Slot>(,<Safe>)(,<Sum>)<CR>
  184. //!,<UNo>(,<SeqNo>),<Sts>,<Errcd>,MMAP,<ExeTime>,<PosData1>,…<PosDataN> ,01:<Result1>…,N:<ResultN>(,<Sum>)<CR>
  185. public override void HandleMMAP(string msg)
  186. {
  187. string[] arrayMsg = msg.Split(',');
  188. string unit = arrayMsg[1];
  189. string seq = arrayMsg[2];
  190. string cmd = arrayMsg[3];
  191. string trs = arrayMsg[4];
  192. string slot = arrayMsg[5];
  193. string sts = 0.ToString("X02");
  194. string Ackcd = 0.ToString("X04");
  195. string leftAll = "";
  196. for (int i = 4; i < arrayMsg.Length - 1; i++)
  197. leftAll += arrayMsg[i] + ",";
  198. string feedback = string.Format(",{0},{1},{2},{3},{4},{5}", unit, seq, sts, Ackcd, cmd, leftAll);
  199. string sum = CheckSum(feedback);
  200. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  201. OnWriteMessage(result);
  202. Thread.Sleep(2000);
  203. string error = 0.ToString("X04");
  204. int executeTime = _rd.Next(10000, 999999);
  205. string exeTime = executeTime.ToString("D6");
  206. string posData = GetPosData();
  207. //bool random = false;
  208. bool full = trs == "C01" || trs == "C02";
  209. bool pre = trs == "C03" || trs == "C04" || trs == "C05" || trs == "C06";
  210. bool mid = trs == "C03" || trs == "C04" ;
  211. bool post = trs == "C05" || trs == "C06";
  212. string mapData = full ? GetMapData(true, true, true) : GetMapData(pre, mid, post);
  213. feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},{7},", unit, seq, sts, error, cmd, exeTime, posData, mapData);
  214. sum = CheckSum(feedback);
  215. result = String.Format("{0}{1}{2}", "!", feedback, sum);
  216. OnWriteMessage(result);
  217. }
  218. public override void HandleRMPD(string msg)
  219. {
  220. Random rd = new Random();
  221. string[] arrayMsg = msg.Split(',');
  222. string unit = arrayMsg[1];
  223. string seq = arrayMsg[2];
  224. string cmd = arrayMsg[3];
  225. string trs = arrayMsg[4];
  226. //string slot = arrayMsg[5];
  227. string sts = 0.ToString("X02");
  228. string Ackcd = 0.ToString("X04");
  229. string error = 0.ToString("X04");
  230. //bool random = false;
  231. bool full = trs == "C01" || trs == "C02";
  232. //string mapData = GetMapData(random, full);
  233. bool pre = trs == "C01" || trs == "C02";
  234. bool mid = trs == "C03" || trs == "C04";
  235. bool post = trs == "C05" || trs == "C06";
  236. string mapData = full ? GetMapData(true, true, true) : GetMapData(pre, mid, post);
  237. if (trs == "C01")
  238. mapData = GetMapThickData(0, 1);
  239. if (trs == "C02")
  240. mapData = GetMapThickData(1, 5);
  241. mapData = GetMapThickData(1, 5);
  242. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, Ackcd, cmd, trs, mapData);
  243. string sum = CheckSum(feedback);
  244. string result = String.Format("{0}{1}{2}", arrayMsg[0], feedback, sum);
  245. OnWriteMessage(result);
  246. }
  247. public override void HandleCSOL(string msg)
  248. {
  249. Response("CSOL", msg);
  250. Thread.Sleep(3000);
  251. string[] arrayMsg = msg.Split(',');
  252. string unit = arrayMsg[1];
  253. string seq = arrayMsg[2];
  254. string sts = 0.ToString("X02");
  255. string error = 0.ToString("X04");
  256. string type = arrayMsg[3];
  257. int executeTime = _rd.Next(10000, 999999);
  258. string exeTime = executeTime.ToString("D6");
  259. string posData = GetPosData();
  260. int status1 = Convert.ToInt32("0011", 2);
  261. string status = string.Format("{0}000", status1.ToString("X1"));
  262. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, type, exeTime, posData);
  263. string sum = CheckSum(feedback);
  264. string result = String.Format("{0}{1}{2}", "!", feedback, sum);
  265. OnWriteMessage(result);
  266. }
  267. public override void HandleMPNT(string msg)
  268. {
  269. Response("MPNT", msg);
  270. Thread.Sleep(3000);
  271. string[] arrayMsg = msg.Split(',');
  272. string unit = arrayMsg[1];
  273. string seq = arrayMsg[2];
  274. string sts = 0.ToString("X02");
  275. string error = 0.ToString("X04");
  276. string type = arrayMsg[3];
  277. int executeTime = _rd.Next(10000, 999999);
  278. string exeTime = executeTime.ToString("D6");
  279. string posData = GetPosData();
  280. int status1 = Convert.ToInt32("0011", 2);
  281. string status = string.Format("{0}000", status1.ToString("X1"));
  282. string feedback = string.Format(",{0},{1},{2},{3},{4},{5},{6},", unit, seq, sts, error, type, exeTime, posData);
  283. string sum = CheckSum(feedback);
  284. string result = String.Format("{0}{1}{2}", "!", feedback, sum);
  285. OnWriteMessage(result);
  286. }
  287. }
  288. }