|
|
@@ -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 常量
|
|
|
+ //最大运动轴数量（abcdefg）
|
|
|
+ private const int MAX_AXIS_NUM = 8;
|
|
|
+ //GalilControllerData最大数据长度
|
|
|
+ private const int MAX_CONTROL_DATA_LENGTH = 264;
|
|
|
+ //GalilAxisData最大数据长度
|
|
|
+ private const int MAX_AXIS_DATA_LENGTH = 28;
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region 内部变量
|
|
|
+ private IByteTransform byteTransform = new BigEndianByteTransformBase();
|
|
|
+ /// <summary>
|
|
|
+ /// Galil数据
|
|
|
+ /// </summary>
|
|
|
+ private GalilControllerData _galilControlData = new GalilControllerData();
|
|
|
+ /// <summary>
|
|
|
+ /// Axis名称字典(key:axisName, value:index)
|
|
|
+ /// </summary>
|
|
|
+ private Dictionary<string, int> _axisNameIndexDic = new Dictionary<string, int>();
|
|
|
+ /// <summary>
|
|
|
+ /// ModuleName
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="port"></param>
|
|
|
+ private string _moduleName;
|
|
|
+ #endregion
|
|
|
+ /// <summary>
|
|
|
+ /// 构造函数
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="port"></param>
|
|
|
+ public GalilSocketSimulator(int port) :base(port)
|
|
|
+ {
|
|
|
+ InitData(port);
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 解析信息
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="data"></param>
|
|
|
+ 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")
|
|
|
+ {
|
|
|
+ //Read
|
|
|
+ response = CreateReadResponse(_galilControlData);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ //Write
|
|
|
+ SetOperation(cmdOperation.Item1, cmdOperation.Item2, cmdOperation.Item3);
|
|
|
+ response = CreateWriteResponse();
|
|
|
+ }
|
|
|
+ OnWriteMessage(response);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ OnWriteMessage(CreateError());
|
|
|
+ }
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 读回复
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="flag"></param>
|
|
|
+ /// <param name="channel"></param>
|
|
|
+ /// <param name="command"></param>
|
|
|
+ /// <param name="registerCount"></param>
|
|
|
+ /// <param name="values"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ private byte[] CreateReadResponse(GalilControllerData data)
|
|
|
+ {
|
|
|
+ //数据头(4 bytes)
|
|
|
+ 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);
|
|
|
+ //数据体(MAX_CONTROL_DATA_LENGTH bytes)
|
|
|
+ Array.Copy(CodeControlData(data), 0, result, 4, MAX_CONTROL_DATA_LENGTH);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 写回复
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="flag"></param>
|
|
|
+ /// <param name="channel"></param>
|
|
|
+ /// <param name="command"></param>
|
|
|
+ /// <param name="startAddress"></param>
|
|
|
+ /// <param name="value"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ private byte[] CreateWriteResponse()
|
|
|
+ {
|
|
|
+ //数据头(1 bytes)
|
|
|
+ int headLength = 1;
|
|
|
+ byte[] result = new byte[headLength];
|
|
|
+ result[0] = 0x3A;
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 错误回复
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="flag"></param>
|
|
|
+ /// <param name="channel"></param>
|
|
|
+ /// <param name="command"></param>
|
|
|
+ /// <param name="error"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ private byte[] CreateError()
|
|
|
+ {
|
|
|
+ int headLength = 1;
|
|
|
+ byte[] result = new byte[headLength];
|
|
|
+ //Command not valid in program
|
|
|
+ result[0] = 0x03;
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 初始化
|
|
|
+ /// </summary>
|
|
|
+ private void InitData(int port)
|
|
|
+ {
|
|
|
+ //初始化AxisData(最大MAX_AXIS_NUM个axis)
|
|
|
+ _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;
|
|
|
+ //加载对应配置文件 GalilControllerCfg-Simulator.xml
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// GalilControllerData编码
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private byte[] CodeControlData(GalilControllerData ctrlData)
|
|
|
+ {
|
|
|
+ byte[] result = new byte[MAX_CONTROL_DATA_LENGTH];
|
|
|
+ int index = 0;
|
|
|
+ //Sample(2 bytes)
|
|
|
+ Array.Copy(byteTransform.GetBytes(ctrlData.Sample), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+ //Inputs(1*10 bytes)
|
|
|
+ Array.Copy(ctrlData.Inputs, 0, result, index, 10);
|
|
|
+ index += 10;
|
|
|
+ //Outputs(1*10 bytes)
|
|
|
+ Array.Copy(ctrlData.Outputs, 0, result, index, 10);
|
|
|
+ index += 10;
|
|
|
+ //Error Code(1 bytes)
|
|
|
+ result[13] = ctrlData.ErrorCode;
|
|
|
+ index ++;
|
|
|
+ //General Status(1 bytes)
|
|
|
+ result[14] = ctrlData.Status;
|
|
|
+ index ++;
|
|
|
+ //S Block(2+2+4=8 bytes)
|
|
|
+ Array.Copy(ctrlData.SBlocks, 0, result, 15, 8);
|
|
|
+ index += 8;
|
|
|
+ //T Block(2+2+4=8 bytes)
|
|
|
+ Array.Copy(ctrlData.TBlocks, 0, result, 23, 8);
|
|
|
+ index += 8;
|
|
|
+ //Axis Datas(28 * 8 bytes)
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// GalilAxisData编码
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="axisData"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ private byte[] CodeAxisData(GalilAxisData axisData)
|
|
|
+ {
|
|
|
+ byte[] result = new byte[MAX_AXIS_DATA_LENGTH];
|
|
|
+ int index = 0;
|
|
|
+ //Status(2 bytes)
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Status), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+ //Switches(1 bytes)
|
|
|
+ result[index] = axisData.Switches;
|
|
|
+ index ++;
|
|
|
+ //Stop Code(1 bytes)
|
|
|
+ result[index] = axisData.StopCode;
|
|
|
+ index ++;
|
|
|
+ //Reference Position(4 bytes)
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.ReferencePosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+ //Motor Position(4 bytes)
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.MotorPosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+ //Position Error
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.PositionError), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+ //Auxiliary Position
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.AuxiliaryPosition), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+ //Velocity
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Velocity), 0, result, index, 4);
|
|
|
+ index += 4;
|
|
|
+ //Torque
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Torque), 0, result, index, 2);
|
|
|
+ index += 2;
|
|
|
+ //Analog
|
|
|
+ Array.Copy(byteTransform.GetBytes(axisData.Res), 0, result, index, 2);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 解析指令(操作符, 运动轴, 操作数)
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="cmd"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// CMD校验
|
|
|
+ /// </summary>
|
|
|
+ /// <returns></returns>
|
|
|
+ private bool CheckCmdValid(string cmdStr)
|
|
|
+ {
|
|
|
+ //长度
|
|
|
+ if(cmdStr.Length < 3) return false;
|
|
|
+ //;结尾
|
|
|
+ if (!cmdStr.EndsWith(";")) return false;
|
|
|
+ //第1位为A~Z
|
|
|
+ if (cmdStr[0] < 'A' || cmdStr[0] > 'Z') return false;
|
|
|
+ //第2位为A~Z
|
|
|
+ if (cmdStr[1] < 'A' || cmdStr[1] > 'Z') return false;
|
|
|
+ if (cmdStr.Length >= 4)
|
|
|
+ {
|
|
|
+ //axis名称为A~H
|
|
|
+ if (cmdStr[2] < 'A' || cmdStr[2] > 'H') return false;
|
|
|
+ //=
|
|
|
+ if(cmdStr.Length > 4 && !cmdStr.Contains("=")) return false;
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 设置操作
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="cmd"></param>
|
|
|
+ /// <param name="axis"></param>
|
|
|
+ /// <param name="value"></param>
|
|
|
+ /// <returns></returns>
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 实时更新电机数据
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="datasDic"></param>
|
|
|
+ private void UpdataRealTimeMotionData(Dictionary<string, CommandMotionData> datasDic)
|
|
|
+ {
|
|
|
+ foreach(var dataItem in datasDic)
|
|
|
+ {
|
|
|
+ if (_axisNameIndexDic.ContainsKey(dataItem.Key))
|
|
|
+ {
|
|
|
+ UpdateAxisData(_axisNameIndexDic[dataItem.Key], dataItem.Value);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 更新对应Axis数据
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="index"></param>
|
|
|
+ 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
|
|
|
+ /// <summary>
|
|
|
+ /// 开/关电机
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="axis"></param>
|
|
|
+ /// <param name="flag"></param>
|
|
|
+ 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);
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 设置目标位置
|
|
|
+ /// </summary>
|
|
|
+ private void SetSpeed(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].Velocity = value;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 设置目标绝对位置
|
|
|
+ /// </summary>
|
|
|
+ private void SetTargetAbsolutePosition(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition = value;
|
|
|
+ }
|
|
|
+ /// <summary>
|
|
|
+ /// 设置目标相对位置
|
|
|
+ /// </summary>
|
|
|
+ private void SetTargetRelativePosition(char axis, int value)
|
|
|
+ {
|
|
|
+ _galilControlData.GalilAxisDatas[axis - 'A'].ReferencePosition += value;
|
|
|
+ }
|
|
|
+ #endregion
|
|
|
+ }
|
|
|
+}
|
