You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1490 lines
51 KiB
C#
1490 lines
51 KiB
C#
using GTN;
|
|
using gts;
|
|
using Newtonsoft.Json;
|
|
using Rs.Framework;
|
|
using Rs.Motion.Base;
|
|
using Rs.Motion.Base.Config;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
|
|
namespace Rs.Motion.GugaoEcat
|
|
{
|
|
public class GugaoAxis:IAxis
|
|
{
|
|
public GugaoAxis(AxisConfig config)
|
|
{
|
|
Config = config;
|
|
}
|
|
|
|
private GugaoCardManager cardManager = GugaoCardManager.Instance;
|
|
//private Logger<AxisGugaoGENImpl> logger = new Logger<AxisGugaoGENImpl>();
|
|
private short apiResult = 0;
|
|
private object m_objLocker = new object();
|
|
private bool m_bIsEncoderSetted = false;
|
|
private short core = 1;
|
|
|
|
|
|
/// <summary>
|
|
/// 轴初始化，设置轴参数
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Init()
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
HomeStatus = EHomeStatus.NotStart;
|
|
if (Config.EnableEncoder == 1)
|
|
{
|
|
//同步EtherCat的位置信息到编码器和规划器
|
|
apiResult = mc_ecat.GTN_GetEcatEncPos(core, (short)Config.AxisId, out int pEncPos);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.AxisInitFail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetPrfPos(core, (short)Config.AxisId, pEncPos);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.AxisInitFail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetEncPos(core, (short)Config.AxisId, pEncPos);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.AxisInitFail;
|
|
}
|
|
|
|
double auEncPos = (double)pEncPos;
|
|
if(Config.AssistEncoder>0)
|
|
{
|
|
apiResult = mc_ecat.GTN_WriteAuEncPos(core, (short)Config.AssistEncoder, ref auEncPos, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.AxisInitFail;
|
|
}
|
|
}
|
|
|
|
//同步
|
|
apiResult = mc_ecat.GTN_SynchAxisPos(core, 1 << Config.AxisId - 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.AxisInitFail;
|
|
}
|
|
HomeStatus = EHomeStatus.Finished;
|
|
}
|
|
IsInitialized = true;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 反初始化轴，停止轴
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Deinit()
|
|
{
|
|
IsInitialized = false;
|
|
m_bIsEncoderSetted = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
else
|
|
{
|
|
Stop();
|
|
Disable();
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 毫米转脉冲
|
|
/// 总长度/电机转一圈走的长度*电机转一圈的脉冲数
|
|
/// </summary>
|
|
/// <param name="mm"></param>
|
|
/// <param name="pulse"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode MmToPulse(double mm, out double pulse)
|
|
{
|
|
pulse = 0;
|
|
|
|
if (Config.DistOneRound == 0.0)
|
|
{
|
|
return ErrorCode.UndefineDistanceOneRound;
|
|
}
|
|
else
|
|
{
|
|
pulse = mm / Config.DistOneRound * (double)Config.PulseOneRound;
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 脉冲转毫米
|
|
/// 总脉冲数/电机转一圈的脉冲数*电机转一圈走的长度
|
|
/// </summary>
|
|
/// <param name="iVal"></param>
|
|
/// <param name="dfDist"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode PulseToMm(double pulse, out double mm)
|
|
{
|
|
mm = 0;
|
|
if (Config.PulseOneRound == 0)
|
|
{
|
|
return ErrorCode.UndefinePulseOneRound;
|
|
}
|
|
else
|
|
{
|
|
mm = pulse / (double)Config.PulseOneRound * Config.DistOneRound;
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 使能电机
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Enable()
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
else
|
|
{
|
|
apiResult = mc_ecat.GTN_ClrSts(core, (short)Config.AxisId, 1);
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
apiResult = mc_ecat.GTN_AxisOn(core, (short)Config.AxisId);
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
else
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 去使能
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Disable()
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
apiResult = mc_ecat.GTN_AxisOff(core, (short)Config.AxisId);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停止运动
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Stop()
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
//int axes = 1 << m_axis_config.AxisId;
|
|
//int option = 1 << m_axis_config.AxisId;
|
|
apiResult = mc_ecat.GTN_Stop(core, 1 << Config.AxisId - 1, 1 << Config.AxisId - 1);//option 对应位1表示紧急停止 0 为平滑停止
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
else
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 运动控制卡回零
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Default_Home_Move()
|
|
{
|
|
//if (!m_motion_card.IsInitialized)
|
|
//{
|
|
// return ErrorCode.CARD_NOT_INIT;
|
|
//}
|
|
//if (!m_bIsInitialized)
|
|
//{
|
|
// return ErrorCode.AXIS_NOT_INIT;
|
|
//}
|
|
//ErrorCode err = MmToPulse(m_axis_config.HomeOffset, out double dfHome_offset);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//err = MmToPulse(1.0, out double dfPulseRaio);
|
|
//m_LastErrorCode = ZTM.ZT_SetHomePol(m_axis_config.CardMc, (short)m_axis_config.AxisId, (short)m_axis_config.HomeOrgLogic);
|
|
//if (m_LastErrorCode != 0)
|
|
//{
|
|
// return ErrorCode.FAIL;
|
|
//}
|
|
//MmToPulse(m_axis_config.HomeSpeed, out double homeSpeedPulse);
|
|
|
|
////acc vel/s mm/s pulse/s pulse/ms mm
|
|
//MmToPulse(m_axis_config.MaxSpeed, out double maxSpeedPulse);
|
|
|
|
//m_LastErrorCode = ZTM.ZT_SetHomeParam(m_axis_config.CardMc,
|
|
// (short)m_axis_config.AxisId,
|
|
// (short)m_axis_config.HomeDir,
|
|
// (short)m_axis_config.HomeMode,
|
|
// (float)((double)(homeSpeedPulse / 4) / 1000.0),
|
|
// (float)((double)homeSpeedPulse / 1000.0),
|
|
// (float)((maxSpeedPulse / 1000.0) / (m_axis_config.AccTime * 1000.0)),
|
|
// (int)dfHome_offset, 0);
|
|
//if (m_LastErrorCode != 0)
|
|
//{
|
|
// return ErrorCode.FAIL;
|
|
//}
|
|
////int axes = SetBit(0, (int)m_axis_config.AxisId, 1);
|
|
////m_LastErrorCode = ZTM.ZT_Home(m_axis_config.CardMc, (uint)axes);
|
|
//if (m_LastErrorCode != 0)
|
|
//{
|
|
// return ErrorCode.MOVE_FAIL;
|
|
//}
|
|
//else
|
|
//{
|
|
// return ErrorCode.OK;
|
|
//}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 绝对位置运动
|
|
/// </summary>
|
|
/// <param name="dfPosVal"></param>
|
|
/// <param name="nSpeedPercent"></param>
|
|
/// <param name="bypassEL"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode MovePos(double dfPosVal, int nSpeedPercent = 100)
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
return ErrorCode.CardNotInit;
|
|
if (!IsInitialized)
|
|
return ErrorCode.AxisNotInit;
|
|
if (!SafeCheck())
|
|
return ErrorCode.Unsafe;
|
|
nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
|
|
nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
|
|
if (HomeStatus != EHomeStatus.Finished)
|
|
return ErrorCode.AxisNotHome;
|
|
if (Config.EnableSoftwareEL == 1 && (dfPosVal > Config.PelSoftwarePosition || dfPosVal < Config.NelSoftwarePosition))//启用软限位
|
|
{
|
|
return ErrorCode.OutOfLimit;
|
|
}
|
|
ErrorCode err = GetEmgStatus(out bool bEmg);
|
|
if (err > ErrorCode.Ok)
|
|
return err;
|
|
if (bEmg)
|
|
return ErrorCode.Emergency;
|
|
err = GetAlarmStatus(out bool bAlarm);
|
|
if (err > ErrorCode.Ok)
|
|
return err;
|
|
if (bAlarm)
|
|
return ErrorCode.Alarm;
|
|
err = IsEnable(out bool bEnable);
|
|
if (err > ErrorCode.Ok)
|
|
return err;
|
|
if (!bEnable)
|
|
return ErrorCode.ServoOff;
|
|
err = IsStop(out bool bStop);
|
|
if (err > ErrorCode.Ok)
|
|
return err;
|
|
if (!bStop)
|
|
return ErrorCode.Moving;
|
|
err = MmToPulse(dfPosVal, out double dfPos);
|
|
if (err > ErrorCode.Ok)
|
|
return err;
|
|
double dfPercent = (double)nSpeedPercent / 100.0;
|
|
|
|
MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
|
|
MmToPulse(Config.MinSpeed, out double minSpeedPulse);
|
|
MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
|
|
//float fAcc = (float)(maxSpeedPulse / (accTime == 0 ? Config.AccTime : accTime) / 1000000.0);
|
|
//float fDec = (float)(maxSpeedPulse / (decTime == 0 ? Config.DecTime : decTime) / 1000000.0);
|
|
|
|
float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);
|
|
float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);
|
|
float fStart = (float)(minSpeedPulse / 1000.0);
|
|
float fStop = (float)(stopSpeedPulse / 1000.0);
|
|
apiResult = mc_ecat.GTN_PrfTrap(core, (short)Config.AxisId);//设置运动模式为点位运动
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetTrapPrm(core, (short)Config.AxisId, out mc_ecat.TTrapPrm trapPrm);//获取点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
trapPrm.acc = fAcc;
|
|
trapPrm.dec = fDec;
|
|
trapPrm.velStart = fStart;
|
|
apiResult = mc_ecat.GTN_SetTrapPrm(core, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetPos(core, (short)Config.AxisId, (int)dfPos);//设置目标位置
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);//设置目标速度
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_Update(core, (1 << Config.AxisId - 1));
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
TargetPosition = dfPosVal;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 相对当前位置运动
|
|
/// </summary>
|
|
/// <param name="dfDistVal"></param>
|
|
/// <param name="nSpeedPercent"></param>
|
|
/// <param name="bypassEL"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode MoveOffset(double dfDistVal, int nSpeedPercent = 100)
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if (!IsInitialized)
|
|
{
|
|
return ErrorCode.AxisNotInit;
|
|
}
|
|
if (!SafeCheck())
|
|
return ErrorCode.Unsafe;
|
|
nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
|
|
nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
|
|
double curPos = 0.0;
|
|
ErrorCode err;
|
|
if (Config.EnableEncoder == 1)
|
|
{
|
|
err = GetEncoderPosition(out curPos);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
err = GetPrfPosition(out curPos);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
}
|
|
double dstPos = curPos + dfDistVal;
|
|
return MovePos(dstPos, nSpeedPercent);
|
|
//bool isNotSafe = !bypassEL && (dstPos > Config.PelSoftwarePosition || dstPos < Config.NelSoftwarePosition)
|
|
// && Config.PelSoftwarePosition != Config.NelSoftwarePosition;
|
|
//if (isNotSafe)
|
|
//{
|
|
// return ErrorCode.POSITION_OUT_OF_RANGE;
|
|
//}
|
|
|
|
err = GetEmgStatus(out bool bEmg);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (bEmg)
|
|
{
|
|
return ErrorCode.Emergency;
|
|
}
|
|
err = GetAlarmStatus(out bool bAlarm);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (bAlarm)
|
|
{
|
|
return ErrorCode.Alarm;
|
|
}
|
|
err = IsEnable(out bool bEnable);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bEnable)
|
|
{
|
|
return ErrorCode.ServoOff;
|
|
}
|
|
err = IsStop(out bool bStop);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bStop)
|
|
{
|
|
return ErrorCode.Moving;
|
|
}
|
|
//return MovePos(dstPos, nSpeedPercent, bypassEL);
|
|
err = MmToPulse(dstPos, out double dfPos);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
double dfPercent = (double)nSpeedPercent / 100.0;
|
|
MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
|
|
MmToPulse(Config.MinSpeed, out double minSpeedPulse);
|
|
MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
|
|
float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);//pulse/ms2
|
|
float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);//pulse/ms2
|
|
float fStart = (float)(minSpeedPulse / 1000.0);//pulse/ms
|
|
float fStop = (float)(stopSpeedPulse / 1000.0);//pulse/ms
|
|
|
|
|
|
apiResult = mc_ecat.GTN_PrfTrap(core, (short)Config.AxisId);//设置运动模式为点位运动
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetTrapPrm(core, (short)Config.AxisId, out mc_ecat.TTrapPrm trapPrm);//获取点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
trapPrm.acc = fAcc;
|
|
trapPrm.dec = fDec;
|
|
trapPrm.velStart = fStart;
|
|
apiResult = mc_ecat.GTN_SetTrapPrm(core, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetPos(core, (short)Config.AxisId, (int)dfPos);//设置目标位置
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);//设置目标速度
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_Update(core, (1 << Config.AxisId - 1));
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
TargetPosition = dstPos;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode MoveJog(short dir, int nSpeedPercent = 10)
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if (!IsInitialized)
|
|
{
|
|
return ErrorCode.AxisNotInit;
|
|
}
|
|
if (!SafeCheck())
|
|
return ErrorCode.Unsafe;
|
|
nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
|
|
nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
|
|
ErrorCode err = IsEnable(out bool bEnable);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bEnable)
|
|
{
|
|
return ErrorCode.ServoOff;
|
|
}
|
|
err = IsStop(out bool bStop);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bStop)
|
|
{
|
|
return ErrorCode.Moving;
|
|
}
|
|
err = GetEmgStatus(out bool bEmg);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (bEmg)
|
|
{
|
|
return ErrorCode.Emergency;
|
|
}
|
|
err = GetAlarmStatus(out bool bAlarm);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (bAlarm)
|
|
{
|
|
return ErrorCode.Alarm;
|
|
}
|
|
else
|
|
{
|
|
double dfPercent = (double)nSpeedPercent / 100.0;
|
|
|
|
MmToPulse(Config.MaxSpeed, out double maxSpeedPulse);
|
|
MmToPulse(Config.MinSpeed, out double minSpeedPulse);
|
|
MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
float fMaxVel = (float)(maxSpeedPulse * dfPercent / 1000.0);//pulse/ms
|
|
float fAcc = (float)(maxSpeedPulse / Config.AccTime / 1000000.0);
|
|
float fDec = (float)(maxSpeedPulse / Config.DecTime / 1000000.0);
|
|
float fStart = (float)(minSpeedPulse / 1000.0);
|
|
float fStop = (float)(stopSpeedPulse / 1000.0);
|
|
|
|
|
|
apiResult = mc_ecat.GTN_PrfJog(core, (short)Config.AxisId);//设置运动模式为JOG运动模式
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetJogPrm(core, (short)Config.AxisId, out mc_ecat.TJogPrm jogPrm);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
jogPrm.acc = fAcc;
|
|
jogPrm.dec = fDec;
|
|
apiResult = mc_ecat.GTN_SetJogPrm(core, (short)Config.AxisId, ref jogPrm);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
if (dir == 0)
|
|
{
|
|
fMaxVel = -1 * fMaxVel;
|
|
}
|
|
apiResult = mc_ecat.GTN_SetVel(core, (short)Config.AxisId, fMaxVel);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_ecat.GTN_Update(core, 1<<Config.AxisId-1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
public override ErrorCode UpdateTargetPosition(double dfNewPosVal)
|
|
{
|
|
//if (!m_motion_card.IsInitialized)
|
|
//{
|
|
// return ErrorCode.CARD_NOT_INIT;
|
|
//}
|
|
//if (!m_bIsInitialized)
|
|
//{
|
|
// return ErrorCode.AXIS_NOT_INIT;
|
|
//}
|
|
//if (_eHomeStatus != EHomeStatus.FINISH)
|
|
//{
|
|
// return ErrorCode.NOT_GO_HOME;
|
|
//}
|
|
//bool isNotSafe = (dfNewPosVal > m_axis_config.PelSoftwarePosition || dfNewPosVal < m_axis_config.NelSoftwarePosition)
|
|
// && m_axis_config.PelSoftwarePosition != m_axis_config.NelSoftwarePosition;
|
|
//if (isNotSafe)
|
|
//{
|
|
// return ErrorCode.POSITION_OUT_OF_RANGE;
|
|
//}
|
|
//ErrorCode err = GetEmgStatus(out bool bEmg);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (bEmg)
|
|
//{
|
|
// return ErrorCode.EMERGENCY;
|
|
//}
|
|
//err = GetAlarmStatus(out bool bAlarm);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (bAlarm)
|
|
//{
|
|
// return ErrorCode.ALARM;
|
|
//}
|
|
//err = IsEnable(out bool bEnable);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (bEnable)
|
|
//{
|
|
// return ErrorCode.SERVO_OFF;
|
|
//}
|
|
//err = MmToPulse(dfNewPosVal, out double dfPos);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//else
|
|
//{
|
|
// m_dfCommandPosition = dfNewPosVal;
|
|
// bool flag19 = m_LastErrorCode != 0;
|
|
// if (flag19)
|
|
// {
|
|
// return ErrorCode.FAIL;
|
|
// }
|
|
// else
|
|
// {
|
|
// return ErrorCode.OK;
|
|
// }
|
|
//}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
// Token: 0x06000256 RID: 598 RVA: 0x0000373C File Offset: 0x0000193C
|
|
public override ErrorCode ChangeSpeed(double dfNewSpeed)
|
|
{
|
|
//if (!m_motion_card.IsInitialized)
|
|
//{
|
|
// return ErrorCode.CARD_NOT_INIT;
|
|
//}
|
|
//if (!m_bIsInitialized)
|
|
//{
|
|
// return ErrorCode.AXIS_NOT_INIT;
|
|
//}
|
|
//if (dfNewSpeed <= m_axis_config.MinSpeed)
|
|
//{
|
|
// dfNewSpeed = m_axis_config.MinSpeed;
|
|
//}
|
|
//if (dfNewSpeed >= m_axis_config.MaxSpeed)
|
|
//{
|
|
// dfNewSpeed = m_axis_config.MaxSpeed;
|
|
//}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断轴是否使能
|
|
/// </summary>
|
|
/// <param name="bIsEable"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode IsEnable(out bool bIsEable)
|
|
{
|
|
bIsEable = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
bIsEable = ((pSts >> 9) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断轴是否回零
|
|
/// </summary>
|
|
/// <param name="bIsHomed"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode IsHomed(out bool bIsHomed)
|
|
{
|
|
bIsHomed = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetEcatHomingStatus(core, 1, out ushort homingStatus);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bool bStatus = homingStatus == 3;
|
|
if (HomeStatus == EHomeStatus.Finished && bStatus)
|
|
{
|
|
bIsHomed = true;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断轴是否已停止
|
|
/// </summary>
|
|
/// <param name="bIsStop"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode IsStop(out bool bIsStop)
|
|
{
|
|
bIsStop = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsStop = ((pSts >> 10) & 0x01) == 0;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断运动是否到位
|
|
/// </summary>
|
|
/// <param name="bIsInPosition"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode IsInPosition(out bool bIsInPosition)
|
|
{
|
|
bIsInPosition = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if (0 == Config.EnableEncoder)//步进
|
|
{
|
|
return IsStop(out bIsInPosition);
|
|
}
|
|
bool bIsStop = false;
|
|
ErrorCode err = IsStop(out bIsStop);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bIsStop)
|
|
{
|
|
return ErrorCode.Ok;
|
|
}
|
|
bool bINP = false;
|
|
err = GetInpStatus(out bINP);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bINP)
|
|
{
|
|
return ErrorCode.Ok;
|
|
}
|
|
double dfCurrentTorlance = 0.0;
|
|
double dfTorlance = Math.Abs(Config.Tolerance);
|
|
err = GetTolerance(out dfCurrentTorlance);
|
|
if (Math.Abs(dfCurrentTorlance) <= dfTorlance)
|
|
{
|
|
bIsInPosition = true;
|
|
}
|
|
return err;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取规划位置
|
|
/// </summary>
|
|
/// <param name="dfCmdPos"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetPrfPosition(out double dfCmdPos)
|
|
{
|
|
dfCmdPos = 0.0;
|
|
ErrorCode err = ErrorCode.Fail;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
try
|
|
{
|
|
lock (m_objLocker)
|
|
{
|
|
apiResult = mc_ecat.GTN_GetPrfPos(core, (short)Config.AxisId, out double pValue, 1, out uint pClock);
|
|
//apiResult = mc_ecat.GTN_ReadAuEncPos(core, (short)Config.AxisId, out double pValue, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
err = PulseToMm(pValue, out dfCmdPos);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
return err;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取编码器位置
|
|
/// </summary>
|
|
/// <param name="dfEncPos"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetEncoderPosition(out double dfEncPos)
|
|
{
|
|
dfEncPos = 0.0;
|
|
ErrorCode err = ErrorCode.Fail;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if (0 == Config.EnableEncoder)
|
|
{
|
|
return GetPrfPosition(out dfEncPos);
|
|
}
|
|
try
|
|
{
|
|
lock (m_objLocker)
|
|
{
|
|
apiResult = mc_ecat.GTN_GetEncPos(core, (short)Config.AxisId, out double pValue, 1, out uint pClock);// ZTM.ZT_GetPosEncoder(m_axis_config.CardMc, (short)m_axis_config.AxisId, ref iPos);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
err = PulseToMm(pValue, out dfEncPos);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
return err;
|
|
}
|
|
|
|
public override ErrorCode GetAuEncoderPosition(out double dfCmdPos)
|
|
{
|
|
dfCmdPos = 0.0;
|
|
ErrorCode err = ErrorCode.Fail;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
try
|
|
{
|
|
lock (m_objLocker)
|
|
{
|
|
apiResult = mc_ecat.GTN_ReadAuEncPos(core, (short)Config.AssistEncoder, out double pValue, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
err = PulseToMm(pValue, out dfCmdPos);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
return err;
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// 获取编码器位置
|
|
/// </summary>
|
|
/// <param name="dfEncPos"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetDriverPosition(out double dfDriverPos)
|
|
{
|
|
dfDriverPos = 0.0;
|
|
ErrorCode err = ErrorCode.Fail;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
try
|
|
{
|
|
lock (m_objLocker)
|
|
{
|
|
apiResult = mc_ecat.GTN_GetEcatEncPos(core, (short)Config.AxisId, out int pValue);// ZTM.ZT_GetPosEncoder(m_axis_config.CardMc, (short)m_axis_config.AxisId, ref iPos);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
err = PulseToMm(pValue, out dfDriverPos);
|
|
}
|
|
}
|
|
catch
|
|
{
|
|
}
|
|
return err;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取轴允许的公差值
|
|
/// </summary>
|
|
/// <param name="dfTolerance"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetTolerance(out double dfTolerance)
|
|
{
|
|
dfTolerance = 0.0;
|
|
ErrorCode err;
|
|
if (0 == Config.EnableEncoder)
|
|
{
|
|
return ErrorCode.Ok;
|
|
}
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
err = GetPrfPosition(out double dfTargetPos);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
err = GetEncoderPosition(out double dfCurrentPos);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
dfTolerance = dfCurrentPos - dfTargetPos;
|
|
double dfTolerance2 = dfCurrentPos - TargetPosition;
|
|
if (Math.Abs(dfTolerance) < Math.Abs(dfTolerance2))
|
|
{
|
|
dfTolerance = dfTolerance2;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取正限位状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetPelStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = ((pSts >> 5) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取负限位状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetNelStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = ((pSts >> 6) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取原点的状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetOrgStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
apiResult = mc_ecat.GTN_GetDi(core, mc_ecat.MC_HOME, out int pValue);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = (pValue >> Config.AxisId-1 & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取报警状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetAlarmStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
apiResult = mc_ecat.GTN_GetDi(core, mc_ecat.MC_ALARM, out int pValue);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = ((pValue >> Config.AxisId - 1) & 0x01) == 1;
|
|
if (bIsOn)
|
|
{
|
|
HomeStatus = EHomeStatus.NotStart;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取EMG状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetEmgStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = ((pSts >> 8) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取到位信号
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetInpStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
apiResult = mc_ecat.GTN_GetSts(core, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bIsOn = ((pSts >> 11) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取EZ状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetEzStatus(out bool bIsOn)
|
|
{
|
|
bIsOn = false;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 轴回原
|
|
/// </summary>
|
|
/// <param name="cat"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode Home()
|
|
{
|
|
//if (_eHomeStatus == EHomeStatus.HOMING || _eHomeStatus == EHomeStatus.START_TO_GO_HOME)
|
|
//{
|
|
// return ErrorCode.OK;
|
|
//}
|
|
//if (!m_bIsInitialized)
|
|
//{
|
|
// return ErrorCode.AXIS_NOT_INIT;
|
|
//}
|
|
//ErrorCode err = GetEmgStatus(out bool bEmg);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (bEmg)
|
|
//{
|
|
// return ErrorCode.EMERGENCY;
|
|
//}
|
|
//err = GetAlarmStatus(out bool bAlarm);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (bAlarm)
|
|
//{
|
|
// return ErrorCode.ALARM;
|
|
//}
|
|
//err = IsEnable(out bool bEnable);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (!bEnable)
|
|
//{
|
|
// return ErrorCode.SERVO_OFF;
|
|
//}
|
|
//err = IsStop(out bool bStop);
|
|
//if (err > ErrorCode.OK)
|
|
//{
|
|
// return err;
|
|
//}
|
|
//if (!bStop)
|
|
//{
|
|
// return ErrorCode.NOT_STOP;
|
|
//}
|
|
//Thread t = t = new Thread(new ParameterizedThreadStart(DealHome));
|
|
//_eHomeStatus = EHomeStatus.START_TO_GO_HOME;
|
|
//t.Name = base.AxisConfig.AxisName + " homing thread";
|
|
//t.Start(this);
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// XY轴回原
|
|
/// </summary>
|
|
/// <param name="obj"></param>
|
|
protected override void DealHome(object obj)
|
|
{
|
|
//try
|
|
//{
|
|
// int step = 0;
|
|
// ErrorCode err;
|
|
// TimeoutUtil timer = new TimeoutUtil();
|
|
// _eHomeStatus = EHomeStatus.HOMING;
|
|
// bool bGoHomePositive = m_axis_config.HomeDir == 1;
|
|
// while (_eHomeStatus != EHomeStatus.ABORT)
|
|
// {
|
|
// Thread.Sleep(50);
|
|
// switch (step)
|
|
// {
|
|
// case 0:
|
|
// {
|
|
// if (IsStop(out bool bStop) > ErrorCode.OK)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (!bStop)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// step++;
|
|
// timer.Restart();
|
|
// break;
|
|
// }
|
|
// case 1:
|
|
// {
|
|
// bool bELSensorOn = false;
|
|
// err = GetOrgStatus(out bool bHomeSensorOn);
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (bGoHomePositive)
|
|
// {
|
|
// err = GetPelStatus(out bELSensorOn);
|
|
// }
|
|
// else
|
|
// {
|
|
// err = GetNelStatus(out bELSensorOn);
|
|
// }
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (Default_Home_Move() > ErrorCode.OK)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// step = 2;
|
|
// timer.Restart();
|
|
// break;
|
|
// }
|
|
// case 2:
|
|
// {
|
|
// err = IsStop(out bool bStop3);
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (bStop3)
|
|
// {
|
|
// step++;
|
|
// timer.Restart();
|
|
// }
|
|
// else
|
|
// {
|
|
// if (timer.IsTimeout(50000L))
|
|
// {
|
|
// Abort_Go_Home();
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// }
|
|
// break;
|
|
// }
|
|
// case 3:
|
|
// {
|
|
// Thread.Sleep(50);
|
|
// if (Zero(0) > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// double speedRatio = 0.2;
|
|
// if (MoveOffset(m_axis_config.HomeOffset, (int)(speedRatio * 100.0), true) > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// step++;
|
|
// timer.Restart();
|
|
// break;
|
|
// }
|
|
// case 4:
|
|
// {
|
|
// err = IsStop(out bool bStop4);
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// err = GetPelStatus(out bool bELSensorOn3);
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// err = GetPelStatus(out bELSensorOn3);
|
|
// if (err > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (bELSensorOn3)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// if (bStop4)
|
|
// {
|
|
// Thread.Sleep(50);
|
|
// if (Zero(0) > ErrorCode.OK)
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// m_dfCommandPosition = 0.0;
|
|
// _eHomeStatus = EHomeStatus.FINISH;
|
|
// return;
|
|
// }
|
|
// else
|
|
// {
|
|
// if (timer.IsTimeout(50000L))
|
|
// {
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// }
|
|
// break;
|
|
// }
|
|
// default:
|
|
// _eHomeStatus = EHomeStatus.FAIL;
|
|
// return;
|
|
// }
|
|
// }
|
|
//}
|
|
//catch (Exception ex)
|
|
//{
|
|
// Debug.WriteLine("IAxis::fn_go_home " + ex.Message);
|
|
//}
|
|
//_eHomeStatus = EHomeStatus.FAIL;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 回原完成
|
|
/// </summary>
|
|
/// <param name="iOrig"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode Zero(int iOrig)
|
|
{
|
|
//if (!IsInitialized)
|
|
//{
|
|
// return ErrorCode.AxisNotInit;
|
|
//}
|
|
//m_dfCommandPosition = 0.0;
|
|
//m_LastErrorCode = ZTM.ZT_SetPosPulse(Config.CardMc, (short)Config.AxisId, iOrig);
|
|
//if (m_LastErrorCode != 0)
|
|
//{
|
|
// return ErrorCode.FAIL;
|
|
//}
|
|
//m_LastErrorCode = ZTM.ZT_SetPosEncoder(Config.CardMc, (short)Config.AxisId, iOrig);
|
|
//if (m_LastErrorCode != 0)
|
|
//{
|
|
// return ErrorCode.FAIL;
|
|
//}
|
|
//else
|
|
//{
|
|
// return ErrorCode.OK;
|
|
//}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
|
|
public void SetHomeFinished()
|
|
{
|
|
Zero(0);
|
|
//base.Set_Home_Finished();
|
|
}
|
|
|
|
|
|
/// <summary>
|
|
/// 获取运动的目标位置
|
|
/// </summary>
|
|
/// <param name="dfTargetPos"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode Get_Target_Position(out double dfTargetPos)
|
|
{
|
|
dfTargetPos = TargetPosition;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode SetPosCompare(ushort channel, double[] postions)
|
|
{
|
|
//apiResult = mc_ecat.GTN_PosCompareStatus(core, (short)channel, out mc_ecat.TPosCompareStatus pstatus);
|
|
ClearCompare(channel);
|
|
//apiResult = mc_ecat.GTN_PosCompareClear(core, (short)channel);
|
|
//if(apiResult!=0)
|
|
//return ErrorCode.Fail;
|
|
//short permit = (short)(1 << channel);
|
|
short[] permit = new short[2];
|
|
permit[0] = 0x02;//第一路位置比较
|
|
permit[1] = 0x04;//第二路位置比较
|
|
apiResult = mc_ecat.GTN_SetTerminalPermitEx(core,0, mc_ecat.MC_HSO, ref permit[0],1, 2);
|
|
LogHelper.Debug($"mc_ecat.GTN_SetTerminalPermitEx({core},0, {mc_ecat.MC_HSO}, {permit},{(short)channel},0x01) ret={apiResult}");
|
|
//if (apiResult != 0)
|
|
//return ErrorCode.Fail;
|
|
//apiResult = mc_ecat.GTN_SetTerminalPermit(core, 0, mc_ecat.MC_HSO, (ushort)(1 << channel));
|
|
//if (apiResult != 0)
|
|
// return ErrorCode.Fail;
|
|
mc_ecat.TPosCompareMode cmpMode;
|
|
apiResult = mc_ecat.GTN_GetPosCompareMode(core, (short)channel, out cmpMode);
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
cmpMode.dimension = 1;
|
|
cmpMode.mode = 0;
|
|
cmpMode.outputMode = 0;
|
|
cmpMode.outputPulseWidth =(ushort)Config.HcmpPulseWidth;
|
|
cmpMode.sourceMode = 2;
|
|
cmpMode.sourceX =(short) Config.AssistEncoder;
|
|
string json=JsonConvert.SerializeObject(cmpMode);
|
|
|
|
apiResult = mc_ecat.GTN_SetPosCompareMode(core, (short)channel, ref cmpMode);
|
|
LogHelper.Debug($"mc_ecat.GTN_SetPosCompareMode({core}, 1 ,ref cmpMode); json={json} ret={apiResult}");
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
List<double> posPulse = new List<double>();
|
|
|
|
foreach (var data in postions)
|
|
{
|
|
GetAuEncoderPosition(out double curPos);
|
|
//GetEncoderPosition(out double curPos);
|
|
//double targetPos = curPos + data;
|
|
double offset = data - curPos;
|
|
MmToPulse(offset, out double pulse);
|
|
posPulse.Add(pulse);
|
|
}
|
|
uint seg = 0;
|
|
|
|
foreach (double data in posPulse)
|
|
{
|
|
mc_ecat.TPosCompareData cmpData = new mc_ecat.TPosCompareData();
|
|
cmpData.hso = 0x03;
|
|
cmpData.gpo = 0x03;
|
|
cmpData.pos = (int)data;
|
|
cmpData.segmentNumber = seg++;
|
|
mc_ecat.GTN_PosCompareData(1, (short)channel, ref cmpData);
|
|
}
|
|
|
|
apiResult = mc_ecat.GTN_PosCompareStart(core, (short)channel);
|
|
if(apiResult!=0)
|
|
return ErrorCode.Fail;
|
|
mc_ecat.GTN_PosCompareSpace(core, (short)channel, out short space);
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode CompareStatus(out short _pStatus, out int _pCount)
|
|
{
|
|
_pStatus = 0;
|
|
_pCount = 0;
|
|
apiResult = mc_ecat.GTN_PosCompareStatus(core, 2, out mc_ecat.TPosCompareStatus status);
|
|
string json = JsonConvert.SerializeObject(status);
|
|
LogHelper.Debug($"mc_ecat.GTN_PosCompareStatus({core}, 2, out mc_ecat.TPosCompareStatus status);status_json{json}");
|
|
//apiResult = mc_pulse.GT_CompareStatus((short)Config.CardId, out _pStatus, out _pCount);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ComparePulse(ushort channel, bool dp = true)
|
|
{
|
|
short value = (short)channel;
|
|
if (value != 0)
|
|
{
|
|
value = (short)(1 << channel);
|
|
|
|
}
|
|
apiResult = mc_ecat.GTN_PosComparePulse(core, 1, 1, 1, 0);
|
|
LogHelper.Debug($"mc_ecat.GTN_PosComparePulse({core}, 1, 1, 1, 0) ret={apiResult}");
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearCompare(ushort uChannel)
|
|
{
|
|
apiResult = mc_ecat.GTN_PosCompareStop(core, (short)uChannel);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
apiResult = mc_ecat.GTN_PosCompareClear(core, (short)uChannel);
|
|
if(apiResult!=0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearAlarm()
|
|
{
|
|
apiResult = mc_ecat.GTN_ClrSts(core, (short)Config.AxisId, 1);//清除轴状态
|
|
if(apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
}
|
|
}
|