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 int[] hds_speeds = new int[] { };
public bool isOpen { get => (long)g_handle != 0; }
public void Open()
{
if (!isOpen) zmotion.ZMC_OpenEth(Adrr, out g_handle);
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 = dValue / 1000;
}
if (valueRESX < 0) valueRESX = /*(float)*/2.5196851;
if (valueRESY < 0) valueRESY = /*(float)*/2.5196851;
if (valueRESZ < 0) valueRESZ = 6.4;
Logs.Write("[HD]send : " + "?*DPOS");
psResponseCode = zmotion.ZMC_DirectCommand(g_handle, "?*DPOS"/*, 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)) 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;
}
///
/// 平台平移（9、 相对运动 相对运动可以转化为绝对运动来实现，这样接口更简单、可靠。） waitmovedone
///
/// 轴移动距离(μm)
/// 轴系数
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;
switch (axisType)
{
case AxisType.X:
valueRES = valueRESX;
//psResponseCode = this.Command_Execute("pPOSX = " + (x * valueRESX + x_Actual), out _);
break;
case AxisType.Y:
valueRES = valueRESY;
//psResponseCode = this.Command_Execute("pPOSY = " + (x * valueRESY + y_Actual), out _);
break;
case AxisType.Z:
valueRES = valueRESZ;
//psResponseCode = this.Command_Execute("pPOSZ = " + (x * valueRESZ + z_Actual), out _);
break;
default:
break;
}
if (x != 0)
{
Int32 psResponseCode = this.Command_Execute("MOVE(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
Console.WriteLine("Move " + +(int)axisType + ": " + psResponseCode/*changps*/);
}
}
}
///
/// 8、 绝对运动
///
///
/// 轴系数
public void To(double x, AxisType axisType)
{
if (!isOpen) return;
if (valueRESX < 0 || valueRESY < 0 || valueRESZ < 0) return;
double valueRES = 1;
switch (axisType)
{
case AxisType.X:
valueRES = valueRESX;
break;
case AxisType.Y:
valueRES = valueRESY;
break;
case AxisType.Z:
valueRES = valueRESZ;
break;
default:
break;
}
this.Command_Execute("MOVEABS(" + Math.Round(x * valueRES) + ") AXIS(" + (int)axisType + ")", out _);
}
///
/// 平台朝一个水平方向连续运动
///
///
/// 正向TRUE 反向FALSE
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));
}
///
/// 取消当前运动和缓冲运动
///
///
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;
double valueRES = 1;
switch (axisType)
{
case AxisType.X:
valueRES = valueRESX;
break;
case AxisType.Y:
valueRES = valueRESY;
break;
case AxisType.Z:
valueRES = valueRESZ;
break;
default:
break;
}
this.Command_Execute("speed(" + (int)axisType + ")=" + (speed * valueRES), out _); ;
}
/* 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指令。 */
///
/// 通用的命令执行接口 当控制器没有缓冲时自动阻赛
///
/// 命令串
/// 接收的字符数组转换为字符串
/// 错误码
public Int32 Command_Execute(string g_command, out string changps)
{
Int32 psResponseCode = -100;
psResponse = new byte[1024];
if (isOpen)
{
psResponseCode = zmotion.ZMC_DirectCommand/*ZMC_Execute*/(g_handle, g_command/*, g_maxtime*/, psResponse, uiResponseLength); // '执行命令并接受应答
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扫描失败。 */
}
}