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.
1651 lines
58 KiB
C#
1651 lines
58 KiB
C#
using CSZTM;
|
|
using GTN;
|
|
using gts;
|
|
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.GugaoPulse
|
|
{
|
|
public class GugaoPulseAxis:IAxis
|
|
{
|
|
public GugaoPulseAxis(AxisConfig config)
|
|
{
|
|
Config = config;
|
|
}
|
|
|
|
private GugaoPulseCardManager cardManager = GugaoPulseCardManager.Instance;
|
|
private short apiResult = 0;
|
|
private double m_dfCommandPosition = 0.0;
|
|
private object m_objLocker = new object();
|
|
|
|
|
|
/// <summary>
|
|
/// 轴初始化，设置轴参数
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Init()
|
|
{
|
|
|
|
mc_pulse_cfg.TAxisConfig axisConfig;
|
|
apiResult = mc_pulse_cfg.GT_GetAxisConfig((short)Config.CardId,(short)Config.AxisId,out axisConfig);
|
|
mc_pulse_cfg.TDiConfig diConfig;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
HomeStatus = EHomeStatus.NotStart;
|
|
if (Config.EnableAlarm == 0)
|
|
{
|
|
mc_pulse.GT_AlarmOff((short)Config.CardId, (short)Config.AxisId);
|
|
}
|
|
else if (Config.EnableAlarm == 1)
|
|
{
|
|
axisConfig.alarmIndex = (short)Config.AxisId;
|
|
apiResult = mc_pulse_cfg.GT_GetDiConfig((short)Config.CardId, mc_pulse.MC_ALARM, (short)Config.AxisId, out diConfig);
|
|
diConfig.active = 1;
|
|
diConfig.reverse = (short)Config.AlarmLogic;
|
|
diConfig.filterTime = 3;
|
|
apiResult = mc_pulse_cfg.GT_SetDiConfig((short)Config.CardId, mc_pulse.MC_ALARM, (short)Config.AxisId, ref diConfig);
|
|
}
|
|
|
|
mc_pulse_cfg.GT_GetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 11, out mc_pulse_cfg.TDoConfig oConfig);
|
|
oConfig.reverse = 0;
|
|
mc_pulse_cfg.GT_SetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 11, ref oConfig);
|
|
|
|
mc_pulse_cfg.GT_GetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 12, out oConfig);
|
|
oConfig.reverse = 1;
|
|
mc_pulse_cfg.GT_SetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 12, ref oConfig);
|
|
|
|
mc_pulse_cfg.GT_GetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 13, out oConfig);
|
|
oConfig.reverse = 1;
|
|
mc_pulse_cfg.GT_SetDoConfig((short)Config.CardId, mc_pulse.MC_GPO, 13, ref oConfig);
|
|
|
|
//先判断正限位
|
|
|
|
|
|
if ((Config.EnableEL & 0x01) == 0)//禁用正限位
|
|
{
|
|
mc_pulse.GT_LmtsOff((short)Config.CardId, (short)Config.AxisId, mc_pulse.MC_LIMIT_POSITIVE);
|
|
}
|
|
else
|
|
{
|
|
axisConfig.limitPositiveIndex = (short)Config.AxisId;
|
|
//axisConfig.alarmIndex = (short)Config.AxisId;
|
|
apiResult = mc_pulse_cfg.GT_GetDiConfig((short)Config.CardId, mc_pulse.MC_LIMIT_POSITIVE, (short)Config.AxisId, out diConfig);
|
|
diConfig.active = 1;
|
|
diConfig.reverse = (short)Config.ElLogic;
|
|
diConfig.filterTime = 3;
|
|
apiResult = mc_pulse_cfg.GT_SetDiConfig((short)Config.CardId, mc_pulse.MC_LIMIT_POSITIVE, (short)Config.AxisId, ref diConfig);
|
|
|
|
|
|
}
|
|
//先判断负限位
|
|
if (((Config.EnableEL >> 1) & 0x01) == 0)
|
|
{
|
|
mc_pulse.GT_LmtsOff((short)Config.CardId, (short)Config.AxisId, mc_pulse.MC_LIMIT_NEGATIVE);
|
|
}
|
|
else
|
|
{
|
|
axisConfig.limitNegativeIndex = (short)Config.AxisId;
|
|
apiResult = mc_pulse_cfg.GT_GetDiConfig((short)Config.CardId, mc_pulse.MC_LIMIT_NEGATIVE, (short)Config.AxisId, out diConfig);
|
|
diConfig.active = 1;
|
|
diConfig.reverse = (short)Config.ElLogic;
|
|
diConfig.filterTime = 3;
|
|
apiResult = mc_pulse_cfg.GT_SetDiConfig((short)Config.CardId, mc_pulse.MC_LIMIT_NEGATIVE, (short)Config.AxisId, ref diConfig);
|
|
}
|
|
apiResult = mc_pulse_cfg.GT_GetDiConfig((short)Config.CardId, mc_pulse.MC_HOME, (short)Config.AxisId, out diConfig);
|
|
diConfig.active = 1;
|
|
diConfig.reverse = (short)Config.HomeOrgLogic;
|
|
diConfig.filterTime = 3;
|
|
apiResult = mc_pulse_cfg.GT_SetDiConfig((short)Config.CardId, mc_pulse.MC_HOME, (short)Config.AxisId, ref diConfig);
|
|
|
|
apiResult = mc_pulse_cfg.GT_SetAxisConfig((short)Config.CardId, (short)Config.AxisId, ref axisConfig);
|
|
// mc_cfg.GT_SetDiConfig(short diType, short diIndex, ref TDiConfig pDi);
|
|
//mc_pulse.GT_EncSns((short)Config.CardId, 0);
|
|
mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);
|
|
IsInitialized = true;
|
|
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 反初始化轴，停止轴
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Deinit()
|
|
{
|
|
IsInitialized = 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_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
apiResult = mc_pulse.GT_AxisOn((short)Config.CardId, (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_pulse.GT_AxisOff((short)Config.CardId, (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_pulse.GT_Stop((short)Config.CardId, 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 = 4)
|
|
{
|
|
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 / 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_pulse.GT_PrfTrap((short)Config.CardId, (short)Config.AxisId);//设置运动模式为点位运动
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_GetTrapPrm((short)Config.CardId, (short)Config.AxisId, out mc_pulse.TTrapPrm trapPrm);//获取点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
trapPrm.acc = fAcc;
|
|
trapPrm.dec = fDec;
|
|
trapPrm.velStart = fStart;
|
|
apiResult = mc_pulse.GT_SetTrapPrm((short)Config.CardId, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)dfPos);//设置目标位置
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetVel((short)Config.CardId, (short)Config.AxisId, fMaxVel);//设置目标速度
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
LogHelper.MoveLog($"acc={trapPrm.acc},dec={trapPrm.dec},targetpos={dfPos},maxvel={fMaxVel}");
|
|
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_pulse.GT_PrfTrap((short)Config.CardId, (short)Config.AxisId);//设置运动模式为点位运动
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_GetTrapPrm((short)Config.CardId, (short)Config.AxisId, out mc_pulse.TTrapPrm trapPrm);//获取点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
trapPrm.acc = fAcc;
|
|
trapPrm.dec = fDec;
|
|
trapPrm.velStart = fStart;
|
|
apiResult = mc_pulse.GT_SetTrapPrm((short)Config.CardId, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)dfPos);//设置目标位置
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetVel((short)Config.CardId, (short)Config.AxisId, fMaxVel);//设置目标速度
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
m_dfCommandPosition = 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_pulse.GT_PrfJog((short)Config.CardId, (short)Config.AxisId);//设置运动模式为JOG运动模式
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_GetJogPrm((short)Config.CardId, (short)Config.AxisId, out mc_pulse.TJogPrm jogPrm);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
jogPrm.acc = fAcc;
|
|
jogPrm.dec = fDec;
|
|
apiResult = mc_pulse.GT_SetJogPrm((short)Config.CardId, (short)Config.AxisId, ref jogPrm);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
if (dir == 0)
|
|
{
|
|
fMaxVel = -1 * fMaxVel;
|
|
}
|
|
apiResult = mc_pulse.GT_SetVel((short)Config.CardId, (short)Config.AxisId, fMaxVel);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_Update((short)Config.CardId, 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_pulse.GT_GetSts((short)Config.CardId, (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>
|
|
/// <returns></returns>
|
|
public void DefaultHome(object obj)
|
|
{
|
|
MessageQueue.Instance.Insert($"{Config.AxisName} go home");
|
|
if (!SafeCheck())
|
|
{
|
|
MessageQueue.Instance.Insert($"{Config.AxisName} go home unsafe");
|
|
return;
|
|
}
|
|
|
|
MessageQueue.Instance.Insert($"{Config.AxisName} go home");
|
|
//判断是否在原点
|
|
GetOrgStatus(out bool bOrg);
|
|
if(bOrg)
|
|
{
|
|
//如果在原点，向回原方向的反方向运动直到离开原点
|
|
if(Config.HomeDir==1)
|
|
{
|
|
MoveJog(0, 2);
|
|
}
|
|
else
|
|
{
|
|
MoveJog(1, 2);
|
|
}
|
|
while (true && bOrg)
|
|
{
|
|
GetOrgStatus(out bOrg);
|
|
}
|
|
Stop();
|
|
Thread.Sleep(100);
|
|
}
|
|
mc_pulse.GT_ZeroPos((short)Config.CardId,(short)Config.AxisId, 1);
|
|
HomeStatus = EHomeStatus.Start;
|
|
MmToPulse(Config.HomeOffset, out double dfHome_offset);
|
|
MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
|
|
mc_pulse.THomePrm homePrm;
|
|
apiResult = mc_pulse.GT_GetHomePrm((short)Config.CardId, (short)Config.AxisId, out homePrm);
|
|
homePrm.mode = (short)Config.HomeMode;
|
|
homePrm.moveDir = (short)Config.HomeDir;
|
|
homePrm.indexDir = (short)(Config.HomeDir * -1);
|
|
homePrm.velHigh = ((double)homeSpeedPulse / 1000.0);
|
|
homePrm.velLow = (float)((double)(homeSpeedPulse / 10) / 1000.0);
|
|
homePrm.acc = ((homeSpeedPulse / 1000.0) / (Config.AccTime * 1000.0));
|
|
homePrm.dec = ((homeSpeedPulse / 1000.0) / (Config.AccTime * 1000.0));
|
|
//homePrm.pad1_1 = 0;
|
|
homePrm.homeOffset = (int)Config.HomeOffset;
|
|
homePrm.searchHomeDistance = 0;
|
|
homePrm.searchIndexDistance = 0;
|
|
homePrm.edge = 0;
|
|
apiResult = mc_pulse.GT_GoHome((short)Config.CardId, (short)Config.AxisId, ref homePrm);
|
|
|
|
// LogHelper.Debug($"axis {Config.AxisName} start home ,homedir={Config.HomeDir},homemode={Config.HomeMode}");
|
|
if (apiResult != 0)
|
|
{
|
|
MessageQueue.Instance.Insert($"{Config.AxisName} go home fail ret:{apiResult}");
|
|
HomeStatus = EHomeStatus.Fail;
|
|
}
|
|
Stopwatch timer = new Stopwatch();
|
|
timer.Restart();
|
|
while (HomeStatus != EHomeStatus.Finished && HomeStatus != EHomeStatus.Abort && timer.ElapsedMilliseconds < 2 * 60 * 1000)
|
|
{
|
|
mc_pulse.GT_GetHomeStatus((short)Config.CardId, (short)Config.AxisId, out mc_pulse.THomeStatus pHomeStatus);
|
|
//MessageQueue.Instance.Insert($"run={pHomeStatus.run},stage={pHomeStatus.stage},err={pHomeStatus.error}");
|
|
Thread.Sleep(10);
|
|
if(pHomeStatus.run==0 && pHomeStatus.stage==100 && pHomeStatus.error==0)
|
|
{
|
|
Thread.Sleep(500);
|
|
mc_pulse.GT_ZeroPos((short)Config.CardId, (short)Config.AxisId, 1);
|
|
mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);
|
|
HomeStatus = EHomeStatus.Finished;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 判断轴是否回零
|
|
/// </summary>
|
|
/// <param name="bIsHomed"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode IsHomed(out bool bIsHomed)
|
|
{
|
|
bIsHomed = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if (HomeStatus == EHomeStatus.Finished)
|
|
{
|
|
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_pulse.GT_GetSts((short)Config.CardId, (short)Config.AxisId, out int pSts, 1, out uint pClock);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
bool warning= ((pSts >> 1) & 0x01) == 1;
|
|
if (warning)
|
|
{
|
|
return ErrorCode.Alarm;
|
|
}
|
|
else
|
|
{
|
|
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_pulse.GT_GetPrfPos((short)Config.CardId, (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_pulse.GT_GetEncPos((short)Config.CardId, (short)Config.AxisId, out double pValue, 1, out uint pClock);
|
|
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.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 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 - m_dfCommandPosition;
|
|
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)
|
|
{
|
|
apiResult = mc_pulse.GT_GetDi((short)Config.CardId, mc_pulse.MC_LIMIT_POSITIVE, out int pValue);
|
|
bIsOn = (pValue >> (Config.AxisId - 1) & 0x01) == 1;
|
|
|
|
// bIsOn = false;
|
|
// //apiResult = mc_pulse.GT_GetDi((short)Config.CardId, mc_ecat.MC_LIMIT_POSITIVE, out int pValue);
|
|
//apiResult = mc_pulse.GT_GetSts((short)Config.CardId, (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)
|
|
{
|
|
apiResult = mc_pulse.GT_GetDi((short)Config.CardId, mc_pulse.MC_LIMIT_NEGATIVE, out int pValue);
|
|
bIsOn = (pValue >> (Config.AxisId - 1) & 0x01) == 1;
|
|
|
|
//bIsOn = false;
|
|
//apiResult = mc_pulse.GT_GetSts((short)Config.CardId, (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_pulse.GT_GetDi((short)Config.CardId, 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_pulse.GT_GetDi((short)Config.CardId, 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_pulse.GT_GetSts((short)Config.CardId, (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_pulse.GT_GetSts((short)Config.CardId, (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 (HomeStatus == EHomeStatus.Homing || HomeStatus == EHomeStatus.Start)
|
|
{
|
|
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(DefaultHome));
|
|
HomeStatus = EHomeStatus.Start;
|
|
t.Name = Config.AxisName + " homing thread";
|
|
t.Start(this);
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
///
|
|
/// </summary>
|
|
//protected void AutoHomeMode(object obj)
|
|
//{
|
|
// bool bIsStop = false;
|
|
// int searchDistance = 1000;//回原搜索距离
|
|
// double searchPulse = 0;
|
|
// double curPos = 0.0;
|
|
// //固高板块的回原点，可以用捕获回原，如果是步进电机采用直接回原的方式
|
|
|
|
|
|
// int pValue = 0;
|
|
// if (Config.HomeDir == 0)
|
|
// {
|
|
// searchDistance *= -1;
|
|
// }
|
|
// MmToPulse(searchDistance, out searchPulse);
|
|
|
|
// MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
|
|
// MmToPulse(Config.MinSpeed, out double minSpeedPulse);
|
|
// MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
// float fMaxVel = (float)(homeSpeedPulse / 1000.0);//pulse/ms
|
|
// float fAcc = (float)(homeSpeedPulse / Config.AccTime / 1000000.0);//pulse/ms2
|
|
// float fDec = (float)(homeSpeedPulse / Config.DecTime / 1000000.0);//pulse/ms2
|
|
// float fStart = (float)(minSpeedPulse / 1000.0);//pulse/ms
|
|
// float fStop = (float)(stopSpeedPulse / 1000.0);//pulse/ms
|
|
|
|
|
|
// mc_pulse.GT_HomeInit((short)Config.CardId);
|
|
// mc_pulse.GT_Home(apiResult, (short)Config.AxisId, (int)searchPulse, fMaxVel, fAcc, 0);
|
|
// while(true)
|
|
// {
|
|
// mc_pulse.GT_HomeSts((short)Config.CardId,(short)Config.AxisId, out ushort pStatus);
|
|
// if(pStatus==1)
|
|
// {
|
|
// ClearAlarm();
|
|
// HomeStatus = EHomeStatus.Finished;
|
|
// break;
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
/// <summary>
|
|
///
|
|
/// </summary>
|
|
/// <param name="obj"></param>
|
|
protected void CaptureHome(object obj)
|
|
{
|
|
if(HomeJudge())
|
|
{
|
|
bool bIsStop = false;
|
|
int searchDistance = 1000;//回原搜索距离
|
|
double searchPulse = 0;
|
|
double curPos = 0.0;
|
|
//固高板块的回原点，可以用捕获回原，如果是步进电机采用直接回原的方式
|
|
|
|
short pStatus = 0;
|
|
int pValue = 0;
|
|
if (Config.HomeDir == 0)
|
|
{
|
|
searchDistance *= -1;
|
|
}
|
|
MmToPulse(searchDistance, out searchPulse);
|
|
mc_pulse.GT_SetCaptureSense((short)Config.CardId, (short)Config.AxisId, mc_pulse.CAPTURE_HOME, (short)Config.HomeOrgLogic);
|
|
//启动捕获回原
|
|
mc_pulse.GT_SetEncPos(0, 1, 0);
|
|
mc_pulse.GT_SetPrfPos(0, 1, 0);
|
|
if (Config.EnableEncoder == 1)
|
|
{
|
|
mc_pulse.GT_GetEncPos(0, 1, out curPos, 1, out uint pClock);
|
|
}
|
|
else
|
|
{
|
|
mc_pulse.GT_GetPrfPos(0, 1, out curPos, 1, out uint pClock);
|
|
}
|
|
apiResult = mc_pulse.GT_SetCaptureMode((short)Config.CardId, (short)Config.AxisId, mc_pulse.CAPTURE_HOME);
|
|
if (apiResult != 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
double targetPos = curPos + searchPulse;
|
|
MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
|
|
MmToPulse(Config.MinSpeed, out double minSpeedPulse);
|
|
MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
float fMaxVel = (float)(homeSpeedPulse / 1000.0);//pulse/ms
|
|
float fAcc = (float)(homeSpeedPulse / Config.AccTime / 1000000.0);//pulse/ms2
|
|
float fDec = (float)(homeSpeedPulse / Config.DecTime / 1000000.0);//pulse/ms2
|
|
float fStart = (float)(minSpeedPulse / 1000.0);//pulse/ms
|
|
float fStop = (float)(stopSpeedPulse / 1000.0);//pulse/ms
|
|
|
|
|
|
apiResult = mc_pulse.GT_PrfTrap((short)Config.CardId, (short)Config.AxisId);//设置运动模式为点位运动
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_GetTrapPrm((short)Config.CardId, (short)Config.AxisId, out mc_pulse.TTrapPrm trapPrm);//获取点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
trapPrm.acc = fAcc;
|
|
trapPrm.dec = fDec;
|
|
trapPrm.velStart = fStart;
|
|
apiResult = mc_pulse.GT_SetTrapPrm((short)Config.CardId, (short)Config.AxisId, ref trapPrm);//设置点位运动参数
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)targetPos);//设置目标位置
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetVel((short)Config.CardId, (short)Config.AxisId, fMaxVel);//设置目标速度
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
|
|
if (apiResult != 0)
|
|
{
|
|
return;// ErrorCode.Fail;
|
|
}
|
|
do
|
|
{
|
|
//获取捕获状态
|
|
apiResult = mc_pulse.GT_GetCaptureStatus((short)Config.CardId, (short)Config.AxisId, out pStatus, out pValue, 1, out uint pClock);
|
|
IsStop(out bIsStop);
|
|
if (bIsStop)
|
|
{
|
|
HomeStatus = EHomeStatus.Fail;
|
|
return;
|
|
}
|
|
} while (pStatus == 0);
|
|
|
|
MmToPulse(Config.HomeOffset, out double offsetPulse);
|
|
targetPos = pValue + (int)offsetPulse;
|
|
apiResult = mc_pulse.GT_SetPos((short)Config.CardId, (short)Config.AxisId, (int)targetPos);//设置目标位置
|
|
apiResult = mc_pulse.GT_Update((short)Config.CardId, (1 << Config.AxisId - 1));
|
|
do
|
|
{
|
|
IsStop(out bIsStop);
|
|
} while (!bIsStop && HomeStatus != EHomeStatus.Abort);
|
|
Thread.Sleep(500);
|
|
Zero(0);
|
|
HomeStatus = EHomeStatus.Finished;
|
|
}
|
|
}
|
|
|
|
protected bool HomeJudge()
|
|
{
|
|
int persent = (int)((Config.HomeSpeed / Config.MaxSpeed) * 100);
|
|
if (persent <= 0)
|
|
persent = 2;
|
|
bool bIsOrg = false;
|
|
bool bIsPel = false;
|
|
bool bIsNel = false;
|
|
int homeStep = 0;
|
|
while(homeStep>=0 && HomeStatus!= EHomeStatus.Abort)
|
|
{
|
|
switch (homeStep)
|
|
{
|
|
case 0://判断是否在原点，在原点，就先离开原点
|
|
GetOrgStatus(out bIsOrg);
|
|
if(bIsOrg)
|
|
{
|
|
//离开原点
|
|
if(Config.HomeDir==0)
|
|
{
|
|
MoveJog(1, persent);
|
|
}
|
|
else
|
|
{
|
|
MoveJog(0, persent);
|
|
}
|
|
do
|
|
{
|
|
GetOrgStatus(out bIsOrg);
|
|
} while (bIsOrg && HomeStatus!= EHomeStatus.Abort);
|
|
Stop();
|
|
Thread.Sleep(500);
|
|
homeStep = -1;
|
|
}
|
|
else
|
|
homeStep = 1;
|
|
break;
|
|
case 1://判断是否在正限位
|
|
GetPelStatus(out bIsPel);
|
|
if( bIsPel)
|
|
{
|
|
//先去找原点
|
|
MoveJog(0, persent);
|
|
do
|
|
{
|
|
GetOrgStatus(out bIsOrg);
|
|
} while (!bIsOrg && HomeStatus != EHomeStatus.Abort);
|
|
Stop();
|
|
ClearAlarm();
|
|
Thread.Sleep(100);
|
|
homeStep = 0;
|
|
}
|
|
else
|
|
homeStep = 2;
|
|
break;
|
|
case 2://判断是否在负限位
|
|
GetNelStatus(out bIsNel);
|
|
if( bIsNel )
|
|
{
|
|
MoveJog(1, persent);
|
|
do
|
|
{
|
|
GetOrgStatus(out bIsOrg);
|
|
} while (!bIsOrg && HomeStatus != EHomeStatus.Abort);
|
|
Stop();
|
|
ClearAlarm();
|
|
Thread.Sleep(100);
|
|
homeStep = 0;
|
|
}
|
|
homeStep = 3;
|
|
break;
|
|
case 3://不在原点，也不在限位上，先朝回原方向搜索
|
|
if(Config.HomeDir==0)
|
|
{
|
|
MoveJog(0, persent);
|
|
|
|
}
|
|
else
|
|
{
|
|
MoveJog(1, persent);
|
|
}
|
|
//搜索的过程中，可能碰到原点，也可能碰到限位,碰到任何一个就停下来
|
|
do
|
|
{
|
|
GetOrgStatus(out bIsOrg);
|
|
if (bIsOrg)
|
|
{
|
|
homeStep = 0;
|
|
}
|
|
GetPelStatus(out bIsPel);
|
|
if (bIsPel)
|
|
{
|
|
homeStep = 1;
|
|
}
|
|
GetNelStatus(out bIsNel);
|
|
if (bIsNel)
|
|
{
|
|
homeStep = 2;
|
|
}
|
|
} while (!bIsOrg && !bIsPel && !bIsNel && HomeStatus != EHomeStatus.Abort);
|
|
Stop();
|
|
Thread.Sleep(100);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
if(HomeStatus!= EHomeStatus.Abort)
|
|
{
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 回原完成
|
|
/// </summary>
|
|
/// <param name="iOrig"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode Zero(int iOrig)
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if(!IsInitialized)
|
|
{
|
|
return ErrorCode.AxisNotInit;
|
|
}
|
|
apiResult = mc_pulse.GT_SetPrfPos((short)Config.CardId, (short)Config.AxisId, 0);
|
|
if(apiResult!=0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
if(Config.EnableEncoder==1)
|
|
{
|
|
apiResult = mc_pulse.GT_SetEncPos((short)Config.CardId, (short)Config.AxisId, 0);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
}
|
|
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 = m_dfCommandPosition;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode SetPosCompare(ushort channel, double[] postions)
|
|
{
|
|
int[] compareDatas = new int[postions.Length];
|
|
int[] companreData2s = new int[3];
|
|
for (int i=0;i<postions.Length;i++)
|
|
{
|
|
GetEncoderPosition(out double curPos);
|
|
double offset = postions[i] - curPos;
|
|
MmToPulse(offset, out double pulse);
|
|
compareDatas[i] = (int)pulse;
|
|
}
|
|
apiResult = mc_pulse.GT_CompareStop((short)Config.CardId);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
if(channel==1)
|
|
{
|
|
apiResult = mc_pulse.GT_CompareData((short)Config.CardId, (short)Config.AxisId, 1, 0, 0, (short)Config.HcmpPulseWidth, ref compareDatas[0], (short)compareDatas.Length, ref companreData2s[0], 0);
|
|
}
|
|
else if(channel==2)
|
|
{
|
|
apiResult = mc_pulse.GT_CompareData((short)Config.CardId, (short)Config.AxisId, 1, 0, 0, (short)Config.HcmpPulseWidth, ref compareDatas[0],0, ref compareDatas[0], (short)compareDatas.Length);
|
|
}
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ComparePulse(ushort channel)
|
|
{
|
|
apiResult = mc_pulse.GT_ComparePulse((short)Config.CardId, (short)channel, 0, (short)Config.HcmpPulseWidth);
|
|
if(apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearCompare(ushort uChannel)
|
|
{
|
|
apiResult = mc_pulse.GT_CompareStop((short)Config.CardId);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearAlarm()
|
|
{
|
|
apiResult = mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);//清除轴状态
|
|
if(apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = mc_pulse.GT_SetDoBit((short)Config.CardId, mc_pulse.MC_ALARM, (short)Config.AxisId, 0);
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
}
|
|
}
|