|
@@ -0,0 +1,401 @@
|
|
|
+using Aitex.Common.Util;
|
|
|
+using Aitex.Core.RT.DataCenter;
|
|
|
+using Aitex.Core.RT.Routine;
|
|
|
+using Aitex.Core.Util;
|
|
|
+using CyberX8_Core;
|
|
|
+using MECF.Framework.Common.CommonData.PUF;
|
|
|
+using MECF.Framework.Common.Device.Festo;
|
|
|
+using MECF.Framework.Common.Device.Galil;
|
|
|
+using MECF.Framework.Common.Net;
|
|
|
+using MECF.Framework.Common.Simulator;
|
|
|
+using MECF.Framework.Simulator.Core.Driver;
|
|
|
+using System;
|
|
|
+using System.Collections.Generic;
|
|
|
+using System.IO;
|
|
|
+using System.Text;
|
|
|
+using Xceed.Wpf.Toolkit.Panels;
|
|
|
+
|
|
|
+namespace CyberX8_Simulator.Devices
|
|
|
+{
|
|
|
+ public class GalilSocketSimulator : SocketDeviceSimulator
|
|
|
+ {
|
|
|
+ #region 常量
|
|
|
+
|
|
|
+ private const int MAX_AXIS_NUM = 8;
|
|
|
+
|
|
|
+ private const int MAX_CONTROL_DATA_LENGTH = 264;
|
|
|
+
|
|
|
+ private const int MAX_AXIS_DATA_LENGTH = 28;
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region 内部变量
|
|
|
+ private IByteTransform byteTransform = new BigEndianByteTransformBase();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private GalilControllerData _galilControlData = new GalilControllerData();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private Dictionary<string, int> _axisNameIndexDic = new Dictionary<string, int>();
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private string _moduleName;
|
|
|
+ #endregion
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ public GalilSocketSimulator(int port) :base(port)
|
|
|
+ {
|
|
|
+ InitData(port);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ protected override void ProcessUnsplitMessage(byte[] data)
|
|
|
+ {
|
|
|
+ string cmdStr = ASCIIEncoding.ASCII.GetString(data);
|
|
|
+ if (CheckCmdValid(cmdStr))
|
|
|
+ {
|
|
|
+ var cmdOperation = AnalyseCommand(cmdStr);
|
|
|
+ byte[] response;
|
|
|
+ if(cmdOperation.Item1 == "QR")
|
|
|
+ {
|
|
|
+
|
|
|
+ response = CreateReadResponse(_galilControlData);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+
|
|
|
+ SetOperation(cmdOperation.Item1, cmdOperation.Item2, cmdOperation.Item3);
|
|
|
+ response = CreateWriteResponse();
|
|
|
+ }
|
|
|
+ OnWriteMessage(response);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ OnWriteMessage(CreateError());
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private byte[] CreateReadResponse(GalilControllerData data)
|
|
|
+ {
|
|
|
+
|
|
|
+ int headLength = 4;
|
|
|
+ byte[] result = new byte[headLength + MAX_CONTROL_DATA_LENGTH];
|
|
|
+ result[0] = 0x00;
|
|
|
+ result[1] = 0x00;
|
|
|
+ short dataLength = (short)(headLength + MAX_CONTROL_DATA_LENGTH);
|
|
|
+ Array.Copy(byteTransform.GetBytes(dataLength), 0, result, 2, 2);
|
|
|
+
|
|
|
+ Array.Copy(CodeControlData(data), 0, result, 4, MAX_CONTROL_DATA_LENGTH);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private byte[] CreateWriteResponse()
|
|
|
+ {
|
|
|
+
|
|
|
+ int headLength = 1;
|
|
|
+ byte[] result = new byte[headLength];
|
|
|
+ result[0] = 0x3A;
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private byte[] CreateError()
|
|
|
+ {
|
|
|
+ int headLength = 1;
|
|
|
+ byte[] result = new byte[headLength];
|
|
|
+
|
|
|
+ result[0] = 0x03;
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void InitData(int port)
|
|
|
+ {
|
|
|
+
|
|
|
+ _galilControlData.GalilAxisDatas = new List<GalilAxisData>();
|
|
|
+ for (int i = 0; i < MAX_AXIS_NUM; i++)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas.Add(new GalilAxisData());
|
|
|
+ _galilControlData.GalilAxisDatas[i].Status = 0x01;
|
|
|
+ }
|
|
|
+ _galilControlData.Inputs = new byte[10];
|
|
|
+ _galilControlData.Outputs = new byte[10];
|
|
|
+ _galilControlData.SBlocks = new byte[8];
|
|
|
+ _galilControlData.TBlocks = new byte[8];
|
|
|
+
|
|
|
+ MotorSimulator.Instance.OnUpdateVariableValueChanged += UpdataRealTimeMotionData;
|
|
|
+
|
|
|
+ string oldXmlPath = PathManager.GetCfgDir();
|
|
|
+ string newXmlPath = oldXmlPath.Replace("CyberX8_Simulator", "CyberX8_RT") + "Devices\\GalilControllerCfg-Simulator.xml";
|
|
|
+ GalilControllerCfg cfg = CustomXmlSerializer.Deserialize<GalilControllerCfg>(new FileInfo(newXmlPath));
|
|
|
+ foreach (GalilDeviceConfig config in cfg.GalilDeviceConfigs)
|
|
|
+ {
|
|
|
+ if (port == config.Port)
|
|
|
+ {
|
|
|
+ _moduleName = config.Module;
|
|
|
+ foreach (GalilAxisConfig item in config.GalilAxises)
|
|
|
+ {
|
|
|
+ _axisNameIndexDic[$"{config.Module}.{item.Name}"] = item.Index;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private byte[] CodeControlData(GalilControllerData ctrlData)
|
|
|
+ {
|
|
|
+ byte[] result = new byte[MAX_CONTROL_DATA_LENGTH];
|
|
|
+ int index = 0;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(ctrlData.Sample), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+
|
|
|
+ Array.Copy(ctrlData.Inputs, 0, result, index, 10);
|
|
|
+ index += 10;
|
|
|
+
|
|
|
+ Array.Copy(ctrlData.Outputs, 0, result, index, 10);
|
|
|
+ index += 10;
|
|
|
+
|
|
|
+ result[13] = ctrlData.ErrorCode;
|
|
|
+ index ++;
|
|
|
+
|
|
|
+ result[14] = ctrlData.Status;
|
|
|
+ index ++;
|
|
|
+
|
|
|
+ Array.Copy(ctrlData.SBlocks, 0, result, 15, 8);
|
|
|
+ index += 8;
|
|
|
+
|
|
|
+ Array.Copy(ctrlData.TBlocks, 0, result, 23, 8);
|
|
|
+ index += 8;
|
|
|
+
|
|
|
+ for(int i = 0;i < MAX_AXIS_NUM; i++)
|
|
|
+ {
|
|
|
+ Array.Copy(CodeAxisData(ctrlData.GalilAxisDatas[i]), 0, result, index, MAX_AXIS_DATA_LENGTH);
|
|
|
+ index += MAX_AXIS_DATA_LENGTH;
|
|
|
+ }
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private byte[] CodeAxisData(GalilAxisData axisData)
|
|
|
+ {
|
|
|
+ byte[] result = new byte[MAX_AXIS_DATA_LENGTH];
|
|
|
+ int index = 0;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Status), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+
|
|
|
+ result[index] = axisData.Switches;
|
|
|
+ index ++;
|
|
|
+
|
|
|
+ result[index] = axisData.StopCode;
|
|
|
+ index ++;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.ReferencePosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.MotorPosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.PositionError), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.AuxiliaryPosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Velocity), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Torque), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Res), 0, result, index, 2);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private (string, char, int) AnalyseCommand(string cmdStr)
|
|
|
+ {
|
|
|
+ var result = ("", ' ', -1);
|
|
|
+
|
|
|
+ result.Item1 = cmdStr.Substring(0, 2);
|
|
|
+
|
|
|
+ if (cmdStr.Length >= 4)
|
|
|
+ {
|
|
|
+ result.Item2 = Convert.ToChar(cmdStr.Substring(2, 1));
|
|
|
+ }
|
|
|
+
|
|
|
+ if (cmdStr.Length >= 5 && int.TryParse(cmdStr.Substring(4, cmdStr.Length - 5), out int tmp))
|
|
|
+ {
|
|
|
+ result.Item3 = tmp;
|
|
|
+ }
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private bool CheckCmdValid(string cmdStr)
|
|
|
+ {
|
|
|
+
|
|
|
+ if(cmdStr.Length < 3) return false;
|
|
|
+
|
|
|
+ if (!cmdStr.EndsWith(";")) return false;
|
|
|
+
|
|
|
+ if (cmdStr[0] < 'A' || cmdStr[0] > 'Z') return false;
|
|
|
+
|
|
|
+ if (cmdStr[1] < 'A' || cmdStr[1] > 'Z') return false;
|
|
|
+ if (cmdStr.Length >= 4)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (cmdStr[2] < 'A' || cmdStr[2] > 'H') return false;
|
|
|
+
|
|
|
+ if(cmdStr.Length > 4 && !cmdStr.Contains("=")) return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private bool SetOperation(string cmd, char axis, int value)
|
|
|
+ {
|
|
|
+ switch (cmd)
|
|
|
+ {
|
|
|
+ case "SH":
|
|
|
+ SwitchMotor(axis, true);
|
|
|
+ break;
|
|
|
+ case "MO":
|
|
|
+ SwitchMotor(axis, false);
|
|
|
+ break;
|
|
|
+ case "PR":
|
|
|
+ SetTargetRelativePosition(axis, value);
|
|
|
+ break;
|
|
|
+ case "PA":
|
|
|
+ SetTargetAbsolutePosition(axis, value);
|
|
|
+ break;
|
|
|
+ case "SP":
|
|
|
+ break;
|
|
|
+ case "AC":
|
|
|
+ break;
|
|
|
+ case "DC":
|
|
|
+ break;
|
|
|
+ case "ST":
|
|
|
+ break;
|
|
|
+ case "BG":
|
|
|
+ break;
|
|
|
+ case "HM":
|
|
|
+ break;
|
|
|
+ case "DP":
|
|
|
+ break;
|
|
|
+ case "DE":
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void UpdataRealTimeMotionData(Dictionary<string, CommandMotionData> datasDic)
|
|
|
+ {
|
|
|
+ foreach(var dataItem in datasDic)
|
|
|
+ {
|
|
|
+ if (_axisNameIndexDic.ContainsKey(dataItem.Key))
|
|
|
+ {
|
|
|
+ UpdateAxisData(_axisNameIndexDic[dataItem.Key], dataItem.Value);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void UpdateAxisData(int index, CommandMotionData data)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[index].Velocity = (int)data.ActualVelocity;
|
|
|
+ _galilControlData.GalilAxisDatas[index].MotorPosition = (int)data.MotorPosition;
|
|
|
+ _galilControlData.GalilAxisDatas[index].ReferencePosition = (int)data.TargetPosition;
|
|
|
+ _galilControlData.GalilAxisDatas[index].Torque = (short)data.ActualTorque;
|
|
|
+ }
|
|
|
+ #region AxisOperation
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void SwitchMotor(char axis, bool flag)
|
|
|
+ {
|
|
|
+ byte[] data = byteTransform.GetBytes(_galilControlData.GalilAxisDatas[axis - 'A'].Status);
|
|
|
+ data[0] &= 0xFE;
|
|
|
+ data[0] |= (byte)(flag ? 0x00 : 0x01);
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].Status = byteTransform.TransUInt16(data, 0);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void SetSpeed(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].Velocity = value;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void SetTargetAbsolutePosition(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition = value;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void SetTargetRelativePosition(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition += value;
|
|
|
+ }
|
|
|
+ #endregion
|
|
|
+ }
|
|
|
+}
|