/// <summary>
/// 判断马达是否停止运动【目标位置和编码器位置差在0.2mm内】
/// </summary>
/// <param name="CardNum"></param>
/// <param name="Axis"></param>
/// <returns></returns>
/// <remarks></remarks>
public static bool ZSPD(short CardNum, short Axis) //'''判断马达是否停止运动
{
bool returnValue = false;
int AxisStatus = 0;
uint temp_pClock = 0;
lock (LockMotion)
PVar.Rtn = gts.GT_GetSts(CardNum, Axis, out AxisStatus, (short)1, out temp_pClock);
if (!(System.Convert.ToBoolean(AxisStatus & 0x400) == true))
{
gts.GT_GetPos(CardNum, Axis, out Tools.AxisTmplPos[CardNum, Axis]);
if (System.Math.Abs(Tools.AxisTmplPos[CardNum, Axis] - Gg.GetEncPos(CardNum, Axis)) > 200)
{
returnValue = false;
}
else
{
returnValue = true;
}
}
else
{
returnValue = false;
}
return(returnValue);
}
public static bool mServoON(short CardTag, short CardNum, short AxisIndex)
{
// CardType CardInfo
//控件 TabIndex 代表卡号， Tag(A,B) A=0代表“固高”，A=1代表其它类型,B代表轴/模块序列号；
//B代表扩展IO序号（0-15）。
int CardType = 0;
switch (CardTag)
{
case 0: //0号卡
case 1:
case 2:
case 3:
case 4:
case 5:
CardType = 0; //固高运动控制卡卡
break;
default: //1号卡
CardType = 1;
break;
}
try
{
switch (CardType)
{
case 0: //固高运动控制卡卡
int Sts = 0;
PVar.Rtn = gts.GT_ClrSts(CardNum, AxisIndex, 1); //清除轴的报警和限位
uint null_UInt32 = 0;
PVar.Rtn = gts.GT_GetSts(CardNum, AxisIndex, out Sts, (short)1, out null_UInt32); //读取轴状态
if (Convert.ToBoolean(Sts & 0x200))
{
//If ZSPD(CardNum, AxisIndex) = False Then
// ShowList(CardNum & "号卡第" & AxisIndex & "轴，正在运动中")
//End If
if (CardNum == 0 && AxisIndex == 3)
{
Gg.SetDo(0, 4, 0);
}
PVar.Rtn = gts.GT_AxisOff(CardNum, AxisIndex);
gts.GT_Stop(CardNum, 1 << (AxisIndex - 1), 0);
GoHome.AxisHome[CardNum, AxisIndex].Step = 0;
}
else
{
PVar.Rtn = gts.GT_AxisOn((short)CardNum, AxisIndex);
PVar.Rtn = gts.GT_SetPrfPos(CardNum, AxisIndex, Gg.GetEncPos((short)CardNum, AxisIndex));
PVar.Rtn = gts.GT_SynchAxisPos((short)CardNum, 1 << ((AxisIndex) - 1)); //将当前轴进行位置同步
uint null_UInt322 = 0;
PVar.Rtn = gts.GT_GetSts(CardNum, AxisIndex, out Sts, (short)1, out null_UInt322); //读取轴状态
if ((Sts & 0x200) == 0)
{
ShowList(BVar.Axisname[CardNum * 8 + AxisIndex - 1] + "伺服ON打开失败");
}
else
{
}
}
break;
case 1:
break;
}
}
catch (Exception)
{
return(false);
}
return(true);
}
/// <summary>
/// GotoHome(short CardNum[卡号], short Axis[轴号], double homeToLimitFDist[极限到原点距离], double searchHomeDist[原点搜索距离], double offsetPos[感应宽度], double homeoffset[原点偏移距离], double vel[搜索原点速度])
/// </summary>
/// <param name="CardNum"></param>
/// <param name="Axis"></param>
/// <param name="homeToLimitFDist"></param>
/// <param name="searchHomeDist"></param>
/// <param name="offsetPos"></param>
/// <param name="homeoffset"></param>
/// <param name="vel"></param>
public static void GotoHome(short CardNum, short Axis, double homeToLimitFDist, double searchHomeDist, double offsetPos, double homeoffset, double vel)
{
uint temp_pClock = 0;
if (GoHome.AxisHome[CardNum, Axis].Step != 0)
{
Frm_Engineering.fEngineering.textBox3.Text = GoHome.AxisHome[CardNum, Axis].Step.ToString();
GoHome.AxisHome[CardNum, Axis].State = true;
switch (GoHome.AxisHome[CardNum, Axis].Step)
{
case 10:
InitTimeH[CardNum, Axis] = new Tool.Delay();
gts.GT_ClrSts(CardNum, Axis, (short)1); //清除报警
gts.GT_SetPrfPos(CardNum, Axis, 0); //规划器置零
gts.GT_SetEncPos(CardNum, Axis, 0); //编码器置零
gts.GT_SynchAxisPos(CardNum, 1 << (Axis - 1)); //将当前轴进行位置同步
GoHome.AxisHome[CardNum, Axis].Counter = 0; //回原点计数
GoHome.AxisHome[CardNum, Axis].Step = 20;
break;
case 20:
GoHome.AxisHome[CardNum, Axis].Capture = 0;
if (Gg.GetHomeDi(CardNum, Axis) == 0) //判断是否在原点上
{
if (GoHome.AxisHome[CardNum, Axis].Counter > 1)
{
GoHome.AxisHome[CardNum, Axis].Step = 140;
}
else
{
GoHome.AxisHome[CardNum, Axis].Counter = GoHome.AxisHome[CardNum, Axis].Counter + 1;
GoHome.AxisHome[CardNum, Axis].Step = 30; //开始搜索原点
}
}
else
{
GoHome.AxisHome[CardNum, Axis].Step = 200; //偏离原点，方向与搜索原点方向相反
}
break;
case 30:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_SetCaptureMode(CardNum, Axis, gts.CAPTURE_HOME); //第一次原点搜索
GoHome.AxisHome[CardNum, Axis].TempPos = 0;
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + searchHomeDist), vel); //启动运动，开始搜索原点
GoHome.AxisHome[CardNum, Axis].Step = 40;
break;
case 40:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
gts.GT_GetCaptureStatus(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].Capture, out GoHome.AxisHome[CardNum, Axis].TempPos, (short)1, out temp_pClock); //获取当前轴原点捕获的状态及捕获的当前位置
if (GoHome.AxisHome[CardNum, Axis].Capture == 1) //判断当前轴是否原点捕获触发
{
//Gg.AxisStop(CardNum, Axis); //当前轴停止
gts.GT_Stop(CardNum, 1 << (Axis - 1), 1 << (Axis - 1));
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Capture = 0;
GoHome.AxisHome[CardNum, Axis].Step = 50;
}
else if (Gg.GetLimitDi_Z(CardNum, Axis) == 1) //判断当前轴是否触发正极限
{
//Gg.AxisStop(CardNum, Axis); //当前轴停止
gts.GT_Stop(CardNum, 1 << (Axis - 1), 1 << (Axis - 1));
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 170;
}
else if (Gg.GetLimitDi_F(CardNum, Axis) == 1) //判断当前轴是否触发负极限
{
//Gg.AxisStop(CardNum, Axis); //当前轴停止
gts.GT_Stop(CardNum, 1 << (Axis - 1), 1 << (Axis - 1));
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 150;
}
else if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false) //判断当前轴规划器是否运动停止（原点搜索距离太小）
{
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 140;
}
break;
case 50:
if (InitTimeH[CardNum, Axis].TimeIsUp(500))
{
GoHome.AxisHome[CardNum, Axis].Step = 52;
}
break;
case 52:
//Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(GoHome.AxisHome[CardNum, Axis].TempPos / Gg.PlusPerUnit(CardNum, Axis)) / Tools.GeerRate[CardNum, Axis] + 1, 2);
//Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + offsetPos), 10); //启动运动，原点反向运动
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + 2), 5); //启动运动，原点反向运动
GoHome.AxisHome[CardNum, Axis].Step = 60;
break;
case 60:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock); //获取当前轴的状态
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false) //判断当前轴是否运动停止
{
GoHome.AxisHome[CardNum, Axis].Step = 70;
}
break;
case 70:
if (InitTimeH[CardNum, Axis].TimeIsUp(500))
{
GoHome.AxisHome[CardNum, Axis].Step = 72;
}
break;
case 72:
gts.GT_SetCaptureMode(CardNum, Axis, gts.CAPTURE_HOME); //第二次原点搜索
//hRtn = gts.GT_SetCaptureMode(CardNum, Axis,gts. CAPTURE_INDEX); //启动当前轴的Z相脉冲捕获
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) - offsetPos * 2), 1); //以1mm/s速度找原点
GoHome.AxisHome[CardNum, Axis].Step = 80; //跳转到下一步
break;
case 80:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock); //获取当前轴的状态
gts.GT_GetCaptureStatus(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].Capture, out GoHome.AxisHome[CardNum, Axis].TempPos, (short)1, out temp_pClock); //获取当前轴Z相脉冲捕获的状态及捕获的当前位置
if (GoHome.AxisHome[CardNum, Axis].Capture == 1) //判断当前轴是否Z相脉冲捕获触发
{
//Gg.AxisStop(CardNum, Axis); //当前轴停止
gts.GT_Stop(CardNum, 1 << (Axis - 1), 1 << (Axis - 1));
GoHome.AxisHome[CardNum, Axis].Capture = 0; //捕获触发标志清零
GoHome.AxisHome[CardNum, Axis].Step = 90;
}
else if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false)
{
GoHome.AxisHome[CardNum, Axis].Step = 140; //跳转到第110步（或脉冲未捕获，回原点结束，回原点失败）
}
break;
case 90:
if (InitTimeH[CardNum, Axis].TimeIsUp(500))
{
GoHome.AxisHome[CardNum, Axis].Step = 92;
}
break;
case 92:
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + homeoffset), 2);
//if (homeoffset < 3)
// {
// Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(GoHome.AxisHome[CardNum, Axis].TempPos / Gg.PlusPerUnit(CardNum, Axis) / Tools.GeerRate[CardNum, Axis]) + homeoffset, 2);
// }
//else
// {
// Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(GoHome.AxisHome[CardNum, Axis].TempPos / Gg.PlusPerUnit(CardNum, Axis) / Tools.GeerRate[CardNum, Axis]) + homeoffset, 5);
// }
GoHome.AxisHome[CardNum, Axis].Step = 100;
break;
case 100:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false) //判断当前轴是否运动停止
{
InitTime.InitialTime();
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 110;
}
break;
case 110:
if (InitTimeH[CardNum, Axis].TimeIsUp(200))
{
GoHome.AxisHome[CardNum, Axis].Step = 120;
}
break;
case 120:
gts.GT_SetPrfPos(CardNum, Axis, 0); //将当前轴规划器位置修改为零点
gts.GT_SetEncPos(CardNum, Axis, 0); //将当前轴编码器位置修改为零点
gts.GT_SynchAxisPos(CardNum, 1 << (Axis - 1)); //将当前轴进行位置同步
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 130;
break;
case 130:
if (InitTimeH[CardNum, Axis].TimeIsUp(50))
{
if (Gg.GetEncPos(CardNum, Axis) == 0 && Gg.GetPrfPos(CardNum, Axis) == 0)
{
GoHome.AxisHome[CardNum, Axis].Result = true;
GoHome.AxisHome[CardNum, Axis].State = false;
GoHome.AxisHome[CardNum, Axis].Step = 0;
}
else
{
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 110;
}
}
break;
case 140:
GoHome.AxisHome[CardNum, Axis].Result = false;
GoHome.AxisHome[CardNum, Axis].State = false;
GoHome.AxisHome[CardNum, Axis].Step = 0;
break;
//*********************************************************************************************************************
case 150: //负极限和原点之间距离
gts.GT_ClrSts(CardNum, Axis, (short)1);
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + homeToLimitFDist), vel);
GoHome.AxisHome[CardNum, Axis].Step = 160;
break;
case 160:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false)
{
GoHome.AxisHome[CardNum, Axis].Step = 20; //重新搜索原点
}
break;
//*********************************************************************************************************************
case 170: //offsetPos大于感应片的宽度
gts.GT_ClrSts(CardNum, Axis, (short)1);
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) - offsetPos), vel);
GoHome.AxisHome[CardNum, Axis].Step = 180;
break;
case 180:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false)
{
GoHome.AxisHome[CardNum, Axis].Step = 20; //重新搜索原点
}
break;
//*********************************************************************************************************************
case 200: //offsetPos大于感应片的宽度
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + offsetPos), vel);
GoHome.AxisHome[CardNum, Axis].Step = 201;
break;
case 201:
if (Gg.GetHomeDi(CardNum, Axis) == 0) //判断是否在原点上
{
Gg.AbsMotion(CardNum, Axis, System.Convert.ToDouble(Gg.GetPrfPosmm(CardNum, Axis) + 1), vel);
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 210;
}
else
{
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false)
{
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 140;
}
}
break;
case 210:
gts.GT_ClrSts(CardNum, Axis, (short)1);
gts.GT_GetSts(CardNum, Axis, out GoHome.AxisHome[CardNum, Axis].status, (short)1, out temp_pClock);
if (System.Convert.ToBoolean(GoHome.AxisHome[CardNum, Axis].status & 0x400) == false)
{
InitTimeH[CardNum, Axis].InitialTime();
GoHome.AxisHome[CardNum, Axis].Step = 220;
}
break;
case (short)220:
if (InitTimeH[CardNum, Axis].TimeIsUp(200))
{
if (Gg.GetHomeDi(CardNum, Axis) == 1)
{
GoHome.AxisHome[CardNum, Axis].Step = 140;
}
else
{
GoHome.AxisHome[CardNum, Axis].Step = 20;
}
}
break;
}
}
}
