Metis_xctg/StageController/HDS/AxisImply.cs

using System;

namespace StageController.HDS
{
    public class AxisImply
    {
        #region Com Connection
        private IntPtr g_handle;
        private float fValue;                    //浮点参数
        private long intType;                     //整型参数
        private const int g_maxtime = 100;       //最大等待时间
        private byte[] psResponse;               //接收的返回的字符
        private UInt32 uiResponseLength = 1024;  //返回字符长度
        public string Adrr = "10.0.0.100";                 //连接的IP地址
        #endregion

        //HDS--> 返回 2，代表运动中，返回 1，代表空闲中
        public int x_State = 0;
        public int y_State = 0;
        public int z_State = 0;
        private double valueRESX = -1;
        private double valueRESY = -1;
        private double valueRESZ = -1;
        public double x_Actual = 0;
        public double y_Actual = 0;
        public double z_Actual = 0;
        public string hds_CONTROL = "";//控制器型号
        public string hds_SERIAL_NUMBER = "";//控制器ID
        public float[] hds_speeds = new float[] { (float)25196.8516, (float)25196.8516, 100 };
        private float max_speed_Z = 100;

        public bool isOpen { get => (long)g_handle != 0; }
        public void Open()
        {
            if (!isOpen) /*zmotion.ZMC_OpenEth(Adrr, out g_handle);*/
            {
                new System.Threading.Tasks.Task(new Action(() =>
                {
                    zmotion.ZMC_OpenEth(Adrr, out g_handle);
                })).Start();
                System.Threading.Thread.Sleep(200);
            }

            if (isOpen)
            {
                string strResult;
                if (this.Command_Execute("Print CONTROL", out strResult) == 0)
                {//返回控制器型号
                    hds_CONTROL = strResult + "";
                }
                if (this.Command_Execute("Print SERIAL_NUMBER", out strResult) == 0)
                {//返回控制器ID。
                    hds_SERIAL_NUMBER = strResult + "";
                }
                ////Console.WriteLine("Cancel Axis: (" + (int)axisType + ") " + psResponseCode/*changps*/);
                //x_speed
                //print speed(0)
                //AxisImply.GetLocalIpAddress("InterNetwork");
            }
        }
        public void Close()
        {
            if (!isOpen) return;

            zmotion.ZMC_Close(g_handle);
            g_handle = (IntPtr)0;
        }
        private string SubStringToChar(byte[] oriResponse)
        {
            string testcode = System.Text.Encoding.ASCII.GetString(oriResponse);
            //先求出最后出现这个字符的下标
            int indexC = testcode.IndexOf('\n');
            if (indexC < 0) indexC = testcode.IndexOf('\0');
            //从下一个索引开始截取
            return testcode.Substring(0, indexC);
        }
        public void RefreshPosition()
        {
            if (!isOpen) return;
            Int32 psResponseCode; psResponse = new byte[1024]; string psResponstring; string[] psResponseArr;

            double dValue;
            if (valueRESX < 0)
            {
                psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?pRESX", psResponse, uiResponseLength);
                if (psResponseCode == 0 && double.TryParse(SubStringToChar(psResponse), out dValue))
                    valueRESX = dValue / 1000;
            }
            if (valueRESY < 0)
            {
                psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?pRESY", psResponse, uiResponseLength);
                if (psResponseCode == 0 && double.TryParse(SubStringToChar(psResponse), out dValue))
                    valueRESY = dValue / 1000;
            }
            if (valueRESZ < 0)
            {
                psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?pRESZ", psResponse, uiResponseLength);
                if (psResponseCode == 0 && double.TryParse(SubStringToChar(psResponse), out dValue))
                    valueRESZ = 1.6;// dValue / 1000;
            }
            if (valueRESX < 0) valueRESX = /*(float)*/2.5196851;
            if (valueRESY < 0) valueRESY = /*(float)*/2.5196851;
            if (valueRESZ < 0) valueRESZ = 1.6;// 6.4;

            //Logs.Write("[HD]send : " + "?*MPOS");
            psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?*MPOS"/*, g_maxtime*/, psResponse, uiResponseLength);
            psResponstring = SubStringToChar(psResponse);
            //Logs.Write("[HD]Recv Data: " + psResponstring);
            psResponseArr = psResponstring.Split(' ');
            if (psResponseCode == 0 && psResponseArr.Length > 2)
            {
                if (float.TryParse(psResponseArr[0], out fValue)) x_Actual = fValue / valueRESX;
                if (float.TryParse(psResponseArr[1], out fValue)) y_Actual = fValue / valueRESY;
                if (float.TryParse(psResponseArr[2], out fValue) && Math.Abs(z_Actual - fValue) > 2) z_Actual = fValue;// / valueRESZ;
            }
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?DPOS[0]", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && float.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Trim(), out fValue))// zmcaux.ZAux_GetDpos/*Mpos*/(g_handle, 0, ref fValue);//zmcaux.ZAux_GetMpos(g_handle, 0, ref fValue);
            //    x_Actual = fValue;// AxisShellList[AxisType.X].Step = fValue;// (m_AxisX.Reversed ? -posInfo.XStep : posInfo.XStep) * m_AxisX.Stepping;
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?DPOS[1]", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && float.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Trim(), out fValue))// zmcaux.ZAux_GetDpos/*ZAux_GetDpos*/(g_handle, 1, ref fValue);//zmcaux.ZAux_GetMpos(g_handle, 1, ref fValue); //### 待调试
            //    y_Actual = fValue;// AxisShellList[AxisType.Y].Step = fValue;// posInfo.YStep * m_AxisY.Stepping;
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?DPOS[2]", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && float.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Trim(), out fValue))// zmcaux.ZAux_GetDpos(g_handle, 2, ref fValue);//zmcaux.ZAux_GetMpos(g_handle, 2, ref fValue);
            //    z_Actual = fValue;// AxisShellList[AxisType.Z].Step = fValue;// posInfo.ZStep * m_AxisZ.Stepping;

            //Logs.Write("[HD]send : " + "?*MTYPE");
            psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?*MTYPE"/*, g_maxtime*/, psResponse, uiResponseLength);
            psResponstring = SubStringToChar(psResponse);
            //Logs.Write("[HD]Recv Data: " + psResponstring);
            psResponseArr = psResponstring.Split(' ');
            if (psResponseCode == 0 && psResponseArr.Length > 2)
            {
                if (long.TryParse(psResponseArr[0], out intType)) x_State = intType != 0 ? 2 : 1;
                if (long.TryParse(psResponseArr[1], out intType)) y_State = intType != 0 ? 2 : 1;
                if (long.TryParse(psResponseArr[2], out intType)) z_State = intType != 0 ? 2 : 1;
            }
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?MTYPE(0)", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && long.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Replace("\07", "").Replace("\n", "").Replace("\0", "").Trim(), out intType))// zmcaux.ZAux_Direct_GetMtype(g_handle, 0, ref intType);
            //    x_State = intType != 0 ? 2 : 1;
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?MTYPE(1)", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && long.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Replace("\07", "").Replace("\n", "").Replace("\0", "").Trim(), out intType))// zmcaux.ZAux_Direct_GetMtype(g_handle, 1, ref intType);
            //    y_State = intType != 0 ? 2 : 1;
            //psResponseCode = zmotion.ZMC_Execute(g_handle, "?MTYPE(2)", g_maxtime, psResponse, uiResponseLength); // '执行命令并接受应答
            //if (psResponseCode == 0 && long.TryParse(System.Text.Encoding.ASCII.GetString(psResponse).Replace("\07", "").Replace("\n", "").Replace("\0", "").Trim(), out intType))// zmcaux.ZAux_Direct_GetMtype(g_handle, 2, ref intType);
            //    z_State = intType != 0 ? 2 : 1;
        }

        /// <summary>
        /// 平台平移（9、 相对运动 相对运动可以转化为绝对运动来实现，这样接口更简单、可靠。） waitmovedone
        /// </summary>
        /// <param name="x">轴移动距离(μm)</param>
        /// <param name="axisType">轴系数</param>
        public void Move(double x, AxisType axisType)
        {
            if (!isOpen) return;
            if (valueRESX < 0 || valueRESY < 0 || valueRESZ < 0) return;
            if (x != 0)
            {
                //相对运动可以转化为绝对运动来实现，这样接口更简单、可靠。
                //Int32 psResponseCode;
                double valueRES = 1;
                Int32 psResponseCode;
                switch (axisType)
                {
                    case AxisType.X:
                        valueRES = valueRESX;
                        psResponseCode = this.Command_Execute("MOVE(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
                        Console.WriteLine("Move " + +(int)axisType + ": " + psResponseCode/*changps*/);
                        //psResponseCode = this.Command_Execute("pPOSX = " + (x * valueRESX + x_Actual), out _);
                        break;
                    case AxisType.Y:
                        valueRES = valueRESY;
                        psResponseCode = this.Command_Execute("MOVE(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
                        Console.WriteLine("Move " + +(int)axisType + ": " + psResponseCode/*changps*/);
                        //psResponseCode = this.Command_Execute("pPOSY = " + (x * valueRESY + y_Actual), out _);
                        break;
                    case AxisType.Z:
                        RefreshPositionZ();
                        Logs.Write("[HD]Sta z_Actual: " + z_Actual);
                        valueRES = valueRESZ;
                        psResponseCode = this.Command_Execute("pPOSZ=" + (x + z_Actual), out _);
                        psResponseCode = this.Command_Execute("runtask 3,movez", out _, true);
                        Console.WriteLine("runtask 3,movez " + +(int)axisType + ": " + psResponseCode/*changps*/);
                        //psResponseCode = this.Command_Execute("pPOSZ = " + (x * valueRESZ + z_Actual), out _);
                        break;
                    default:
                        break;
                }//结合 虚拟轴控制的特征 优化 数据精度//# 待确认
            }
        }
        public void RefreshPositionZ()
        {
            if (!isOpen) return;
            Int32 psResponseCode; psResponse = new byte[1024]; string psResponstring; string[] psResponseArr;
            double dValue;
            if (valueRESZ < 0)
            {
                psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?pRESZ", psResponse, uiResponseLength);
                if (psResponseCode == 0 && double.TryParse(SubStringToChar(psResponse), out dValue))
                    valueRESZ = 1.6;// dValue / 1000;
            }
            if (valueRESZ < 0) valueRESZ = 1.6;// 6.4;
            //Logs.Write("[HD]send : " + "?*MPOS");
            psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?*MPOS"/*, g_maxtime*/, psResponse, uiResponseLength);
            psResponstring = SubStringToChar(psResponse);
            //Logs.Write("[HD]Recv Data: " + psResponstring);
            psResponseArr = psResponstring.Split(' ');
            if (psResponseCode == 0 && psResponseArr.Length > 2)
                if (float.TryParse(psResponseArr[2], out fValue) && Math.Abs(z_Actual - fValue) > 2) z_Actual = fValue;// / valueRESZ;
        }
        /// <summary>
        /// 8、 绝对运动
        /// </summary>
        /// <param name="x"></param>
        /// <param name="axisType">轴系数</param>
        public void To(double x, AxisType axisType)
        {
            if (!isOpen) return;
            if (valueRESX < 0 || valueRESY < 0 || valueRESZ < 0) return;
            double valueRES = 1;
            Int32 psResponseCode;
            switch (axisType)
            {
                case AxisType.X:
                    valueRES = valueRESX;
                    this.Command_Execute("MOVEABS(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
                    break;
                case AxisType.Y:
                    valueRES = valueRESY;
                    this.Command_Execute("MOVEABS(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
                    break;
                case AxisType.Z:
                    valueRES = valueRESZ;
                    RefreshPositionZ();
                    Logs.Write("[HD]Sta z_Actual: " + z_Actual);
                    psResponseCode = this.Command_Execute("pPOSZ=" + x, out _);
                    psResponseCode = this.Command_Execute("runtask 3,movez", out _, true);
                    Console.WriteLine("runtask 3,movez " + +(int)axisType + ": " + psResponseCode/*changps*/);
                    break;
                default:
                    break;
            }
        }
        /// <summary>
        /// 平台朝一个水平方向连续运动
        /// </summary>
        /// <param name="axisType"></param>
        /// <param name="isPositive">正向TRUE 反向FALSE</param>
        public void VMove(AxisType axisType, bool isPositive)
        {
            if (!isOpen) return;
            this.Command_Execute("VMOVE(" + (isPositive ? 1 : -1) + ") AXIS(" + (int)axisType + ")", out _);
            Console.WriteLine("VMove Axis: (" + (int)axisType + ") " + (isPositive ? 1 : -1));
        }
        /// <summary>
        /// 取消当前运动和缓冲运动
        /// </summary>
        /// <param name="axisType"></param>
        public void CancelCommand(AxisType axisType)
        {
            if (!isOpen) return;
            Int32 psResponseCode = this.Command_Execute("CANCEL(2) AXIS(" + (int)axisType + ")", out _);
            Console.WriteLine("Cancel Axis: (" + (int)axisType + ") " + psResponseCode/*changps*/);
        }

        public void SetSpeed(float speed, AxisType axisType)
        {
            if (!isOpen) return;
            if (valueRESX < 0 || valueRESY < 0 || valueRESZ < 0) return;
            switch (axisType)
            {
                case AxisType.X:
                    hds_speeds[0] = speed;// * valueRES;
                    this.Command_Execute("speed(" + (int)axisType + ")=" + (speed * valueRESX), out _); ;
                    break;
                case AxisType.Y:
                    hds_speeds[1] = speed;// * valueRES;
                    this.Command_Execute("speed(" + (int)axisType + ")=" + (speed * valueRESY), out _); ;
                    break;
                case AxisType.Z:
                    if (speed > max_speed_Z) speed = max_speed_Z;
                    if (hds_speeds[2] == speed) return;
                    hds_speeds[2] = speed;// * valueRES;  valueRES = valueRESZ;
                    this.Command_Execute("pSPZ=" + (speed/* * valueRES*/ * 0.001), out _, true);
                    this.Command_Execute("runtask 3,SPZ", out _, true);
                    break;
                default:
                    break;
            }
        }








        /* ZBasic具有实时多任务特性，多个ZBasic程序可以同时构建并同时运行，使得复杂的应用变得简单易行   通过PC在线发送BASIC命令也可以实现同样的效果，控制器的BASIC程序和PC在线BASIC命令可以同时运行         BASE(0,1,2,3)’轴列表为：0，1，2，3 ;ZMC004   MOVE(100,100,100,100)            ’轴0，1，2，3运动
        MOVE(1000) AXIS(1)’ 轴1运动1000unit      BASE(0,1)   DEFPOS（0，0） ‘设置轴0和轴1当前点为零点.         VMOVE 描述：连续往一个方向运动，当前面的VMOVE运动没有停止时，此VMOVE指令会自动替换前面的VMOVE指令并修改方向，因此无需CANCEL前面的VMOVE指令。 */
        /// <summary>
        /// 通用的命令执行接口  当控制器没有缓冲时自动阻赛
        /// </summary>
        /// <param name="g_command">命令串</param>
        /// <param name="changps">接收的字符数组转换为字符串</param>
        /// <returns>错误码</returns>
        public Int32 Command_Execute(string g_command, out string changps, bool ZMC_Execute_Status = false)
        {
            Int32 psResponseCode = -100;
            psResponse = new byte[1024];
            if (isOpen)
            {
                Logs.Write("[HD]send : " + g_command);
                if (ZMC_Execute_Status)
                {
                    psResponseCode = 0; zmotion./*ZMC_DirectCommand*/ZMC_ExecuteNoAck(g_handle, g_command, g_maxtime/*, psResponse, uiResponseLength*/); // '执行命令并接受应答
                }
                else
                    psResponseCode = zmotion.ZMC_DirectCommand/*ZMC_Execute*/(g_handle, g_command/*, g_maxtime*/, psResponse, uiResponseLength); // '执行命令并接受应答
                Logs.Write("[HD]Recv Code: " + psResponseCode);
                changps = System.Text.Encoding.ASCII.GetString(psResponse);
                //TextBox2.Text = TextBox2.Text + ">>" + g_command + "\r\n";
                if (psResponse[0] != '\0')                 //   '显示接收信息
                    psResponseCode = 0;
                //    TextBox2.Text = TextBox2.Text + changps + "\r\n";
            }
            else
                changps = "Connect First!";
            //TextBox2.Text = TextBox2.Text + "Connect First!" + "\r\n";
            return psResponseCode;
        }
        /*  c) 命令下发要注意防止拥堵，例如采用线程的同步。         当前各轴的分辨率可通过指令去查询，例如查询 X 轴用?pRESX   反馈的数据单位为 count，要转换为 mm 为单位，则需要将该反馈值除以分辨率      HALT 是停止所有任务。 程序中禁止使用，防止出现异常！！！*/
        /*  文档“ZBasic.chm”;   设置轴的反向间隙，扩展轴无效。   BACKLASH(0) '关闭反向间隙功能。   BACKLASH(1, 0.1)      '设置反向间隙0.1mm   设置轴的螺距补偿，扩展轴无效。   每点的补偿脉冲个数存储在TABLE表里面。
        DPOS  轴的虚拟坐标位置，或称需求位置。 写DPOS会自动转换为OFFPOS偏移，不会移动电机。 以UNITS作为单位。
        FE    随动误差，该值等于DPOS-MPOS。
        FS_LIMIT  正向软限位位置，单位是units。   取消设置时，只需要把值设大些。   FS_LIMIT=2000000    ‘取消正向软限位设置
        MTYPE  当前正在进行的运动指令类型。  0 IDLE (没有运动)  10 FORWARD
        print CONTROL  返回控制器型号。           464   303
        print DATE     设置系统日期，或是返回2000年以后的天数。      7782  7800
        print DATE$    字符串函数，按DD/MM/YYYY格式返回当前日期。    22:04:2021  10:05:2021
        PRINT IP_ADDRESS 控制器IP地址。  只有带以太网接口的控制器支持，读取时以32位整数返回，见例一。  16777343 = 127.0.0.1。  1677721610 = 10.0.0.100
        PRINT IP_GATEWAY 控制器IP网关。                                                                               16820416
        PRINT IP_NETMASK 控制器IP网络掩码。                                                                             16777215
        Print SERIAL_NUMBER 返回控制器ID。  是一个唯一的序列号，生成ZAR包的时候也可以和这个ID绑定，这样这个ZAR只能为这个控制器所用。  190101376
        ?*version 系统软件版本号。                                                                                                          2.9400 
        SLOT_SCAN  总线扫描，通过RETURN返回成功与否，-1扫描成功 0扫描失败。   */

    }
}