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.
1447 lines
49 KiB
C#
1447 lines
49 KiB
C#
using csLTDMC;
|
|
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;
|
|
using static gts.mc_pulse;
|
|
|
|
namespace Rs.Motion.Leisai
|
|
{
|
|
public class LeisaiAxis:IAxis
|
|
{
|
|
public LeisaiAxis(AxisConfig config)
|
|
{
|
|
Config = config;
|
|
}
|
|
|
|
private LeisaiCardManager cardManager = LeisaiCardManager.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()
|
|
{
|
|
//设置回原点信号的有效电平
|
|
apiResult = LTDMC.dmc_set_home_pin_logic(Config.CardId, Config.AxisId, Config.HomeOrgLogic, 0);
|
|
if(apiResult!=0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = LTDMC.dmc_set_home_pin_logic({Config.CardId}, {Config.AxisId}, {Config.HomeOrgLogic}, 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 = LTDMC.dmc_write_sevon_pin(Config.CardId, Config.AxisId, 0);
|
|
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 = LTDMC.dmc_write_sevon_pin(Config.CardId, Config.AxisId, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停止运动
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public override ErrorCode Stop()
|
|
{
|
|
bool smoothStop = true;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
if(smoothStop)
|
|
{
|
|
apiResult = LTDMC.dmc_stop(Config.CardId, Config.AxisId, 0);
|
|
}
|
|
else
|
|
{
|
|
apiResult = LTDMC.dmc_stop(Config.CardId, Config.AxisId, 1);
|
|
}
|
|
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;
|
|
mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);//清除轴状态
|
|
//运动之前先清一下状态
|
|
//ClearAlarm();
|
|
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 );//pulse/ms
|
|
float fStart = (float)(minSpeedPulse * dfPercent);
|
|
float fStop = (float)(stopSpeedPulse * dfPercent);
|
|
apiResult = LTDMC.dmc_set_profile(Config.CardId,Config.AxisId, fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = LTDMC.dmc_set_s_profile(Config.CardId, Config.AxisId, 0,Config.STime);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
apiResult = LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)dfPos, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
m_dfCommandPosition = 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 offset, int nSpeedPercent = 100)
|
|
{
|
|
if (!cardManager.IsInitialized)
|
|
return ErrorCode.CardNotInit;
|
|
if (!IsInitialized)
|
|
return ErrorCode.AxisNotInit;
|
|
if (!SafeCheck())
|
|
return ErrorCode.Unsafe;
|
|
//运动之前先清一下状态
|
|
//ClearAlarm();
|
|
nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
|
|
nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
|
|
//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(offset, 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);//pulse/ms
|
|
float fStart = (float)(minSpeedPulse * dfPercent);
|
|
float fStop = (float)(stopSpeedPulse * dfPercent);
|
|
apiResult = LTDMC.dmc_set_profile(Config.CardId, Config.AxisId, fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = LTDMC.dmc_set_s_profile(Config.CardId, Config.AxisId, 0, Config.STime);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
apiResult = LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)dfPos, 0);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
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;
|
|
mc_pulse.GT_ClrSts((short)Config.CardId, (short)Config.AxisId, 1);//清除轴状态
|
|
//运动之前先清一下状态
|
|
//ClearAlarm();
|
|
nSpeedPercent = nSpeedPercent <= 0 ? 10 : nSpeedPercent;
|
|
nSpeedPercent = nSpeedPercent > 100 ? 100 : nSpeedPercent;
|
|
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;
|
|
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);//pulse/ms
|
|
float fStart = (float)(minSpeedPulse * dfPercent);
|
|
float fStop = (float)(stopSpeedPulse * dfPercent);
|
|
apiResult = LTDMC.dmc_set_profile(Config.CardId, (ushort)(Config.AxisId), fStart, fMaxVel, Config.AccTime, Config.DecTime, fStop);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = LTDMC.dmc_set_s_profile(Config.CardId, (ushort)(Config.AxisId), 0, Config.STime);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
apiResult = LTDMC.dmc_vmove(Config.CardId, (ushort)(Config.AxisId), (ushort)dir);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
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 = LTDMC.dmc_read_sevon_pin(Config.CardId, Config.AxisId);
|
|
bIsEable = apiResult == 0;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 运动控制卡回零
|
|
/// </summary>
|
|
/// <returns></returns>
|
|
public void DefaultHome(object obj)
|
|
{
|
|
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 (bOrg)
|
|
{
|
|
GetOrgStatus(out bOrg);
|
|
}
|
|
Stop();
|
|
Thread.Sleep(100);
|
|
}
|
|
apiResult = LTDMC.dmc_set_position(Config.CardId, Config.AxisId, 0);
|
|
apiResult = LTDMC.dmc_set_encoder(Config.CardId, Config.AxisId, 0);
|
|
HomeStatus = EHomeStatus.Start;
|
|
|
|
MmToPulse(Config.HomeOffset, out double dfHome_offset);
|
|
MmToPulse(Config.HomeSpeed, out double homeSpeedPulse);
|
|
MmToPulse(Config.StopSpeed, out double stopSpeedPulse);
|
|
|
|
apiResult = LTDMC.dmc_set_profile(Config.CardId, Config.AxisId, homeSpeedPulse / 2, homeSpeedPulse, Config.AccTime, Config.DecTime, stopSpeedPulse);
|
|
|
|
apiResult = LTDMC.dmc_set_homemode(Config.CardId, Config.AxisId, (ushort)Config.HomeDir, Config.HomeVelMode, Config.HomeMode, Config.EnableEncoder);
|
|
|
|
LTDMC.dmc_home_move(Config.CardId, Config.AxisId);
|
|
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)
|
|
{
|
|
|
|
apiResult = LTDMC.dmc_check_done(Config.CardId, Config.AxisId);
|
|
Thread.Sleep(10);
|
|
if(apiResult==1)
|
|
{
|
|
GetEncoderPosition(out double curPos);
|
|
MmToPulse(Config.HomeOffset, out double offsetPulse);
|
|
MoveOffset(Config.HomeOffset, 5);
|
|
//LTDMC.dmc_pmove(Config.CardId, Config.AxisId, (int)offsetPulse, 0);
|
|
do
|
|
{
|
|
IsStop(out bool stoped);
|
|
if (stoped)
|
|
break;
|
|
}
|
|
while (true);
|
|
Thread.Sleep(500);
|
|
Zero(0);
|
|
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 = LTDMC.dmc_check_done(Config.CardId, Config.AxisId);
|
|
|
|
bIsStop = apiResult == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode IsArrived(out bool bIsArrived)
|
|
{
|
|
bIsArrived = false;
|
|
if (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
bool bIsStop = false;
|
|
ErrorCode err = IsStop(out bIsStop);
|
|
if (err > ErrorCode.Ok)
|
|
{
|
|
return err;
|
|
}
|
|
if (!bIsStop)
|
|
{
|
|
return ErrorCode.Ok;
|
|
}
|
|
double currentPos = 0.0;
|
|
if(Config.EnableEncoder==1)
|
|
{
|
|
GetEncoderPosition(out currentPos);
|
|
}
|
|
else
|
|
{
|
|
GetPrfPosition(out currentPos);
|
|
}
|
|
bIsArrived = Math.Abs((currentPos - m_dfCommandPosition)) < 0.05;
|
|
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;
|
|
}
|
|
if(Config.EnableInp==1)
|
|
{
|
|
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)
|
|
{
|
|
Int32 pulse = LTDMC.dmc_get_position(Config.CardId, Config.AxisId);
|
|
err = PulseToMm(pulse, 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)
|
|
{
|
|
int paulse = LTDMC.dmc_get_encoder(Config.CardId, Config.AxisId);
|
|
err = PulseToMm(paulse, 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 bIsInPel)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsInPel = ((status >> 1) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取负限位状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetNelStatus(out bool bIsInNel)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsInNel = ((status >> 2) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取原点的状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetOrgStatus(out bool bIsInOrg)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsInOrg = ((status >> 4) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取报警状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetAlarmStatus(out bool bIsAlarm)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsAlarm = ((status >> 0) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取EMG状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetEmgStatus(out bool bIsEmg)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsEmg = ((status >> 3) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取到位信号
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetInpStatus(out bool bIsInp)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsInp = ((status >> 8) & 0x01) == 1;
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 获取EZ状态
|
|
/// </summary>
|
|
/// <param name="bIsOn"></param>
|
|
/// <returns></returns>
|
|
public override ErrorCode GetEzStatus(out bool bIsInEz)
|
|
{
|
|
uint status = LTDMC.dmc_axis_io_status(Config.CardId, Config.AxisId);
|
|
bIsInEz = ((status >> 9) & 0x01) == 1;
|
|
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 (!cardManager.IsInitialized)
|
|
{
|
|
return ErrorCode.CardNotInit;
|
|
}
|
|
ErrorCode 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;
|
|
}
|
|
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 = LTDMC.dmc_set_position(Config.CardId, Config.AxisId, 0);
|
|
if(apiResult!=0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
if(Config.EnableEncoder==1)
|
|
{
|
|
apiResult = LTDMC.dmc_set_encoder(Config.CardId, 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)
|
|
{
|
|
Int32 remained_points = 0;
|
|
Int32 current_point = 0;
|
|
Int32 runned_points = 0;
|
|
apiResult = LTDMC.dmc_hcmp_get_current_state(Config.CardId, channel, ref remained_points, ref current_point, ref runned_points);
|
|
|
|
apiResult = LTDMC.dmc_hcmp_set_mode(Config.CardId, channel, 4);
|
|
if(apiResult!=0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareStop({(short)Config.CardId}, {(short)(channel - 1)});");
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = LTDMC.dmc_hcmp_set_config(Config.CardId, channel, Config.AxisId, 1, 0, Config.HcmpPulseWidth);
|
|
if(apiResult!=0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareMode({(short)Config.CardId}, {(short)(channel - 1)}, mc_pulse.COMPARE2D_MODE_1D);");
|
|
return ErrorCode.Fail;
|
|
}
|
|
apiResult = LTDMC.dmc_hcmp_clear_points(Config.CardId, channel);
|
|
if (apiResult != 0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = LTDMC.dmc_hcmp_clear_points({Config.CardId}, {channel});");
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
foreach (double pos in postions)
|
|
{
|
|
double tarPos = 0.0;
|
|
MmToPulse(pos, out tarPos);
|
|
apiResult = LTDMC.dmc_hcmp_add_point(Config.CardId, channel, (int)tarPos);
|
|
if (apiResult != 0)
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode CompareStatus(out short _pStatus,out int _pCount)
|
|
{
|
|
_pStatus = 0;
|
|
_pCount = 0;
|
|
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ComparePulse(ushort channel,bool dp=true)
|
|
{
|
|
|
|
apiResult = LTDMC.dmc_write_cmp_pin(Config.CardId, channel, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = LTDMC.dmc_write_cmp_pin({Config.CardId}, {channel}, 1);");
|
|
return ErrorCode.Fail;
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearCompare(ushort uChannel)
|
|
{
|
|
apiResult = LTDMC.dmc_hcmp_set_mode(Config.CardId, uChannel, 0);
|
|
if (apiResult != 0)
|
|
{
|
|
return ErrorCode.Fail;
|
|
}
|
|
else
|
|
{
|
|
apiResult = LTDMC.dmc_hcmp_clear_points(Config.CardId, uChannel);
|
|
if (apiResult != 0)
|
|
{
|
|
LogHelper.Debug($"{apiResult} = LTDMC.dmc_hcmp_clear_points({Config.CardId}, {uChannel});");
|
|
return ErrorCode.Fail;
|
|
}
|
|
}
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
public override ErrorCode ClearAlarm()
|
|
{
|
|
apiResult = LTDMC.dmc_write_erc_pin(Config.CardId, Config.AxisId, 1);
|
|
if (apiResult != 0)
|
|
{
|
|
LogHelper.Debug($"{apiResult}= LTDMC.dmc_write_erc_pin({Config.CardId},{Config.AxisId}, 1);");
|
|
return ErrorCode.Fail;
|
|
}
|
|
|
|
return ErrorCode.Ok;
|
|
}
|
|
|
|
}
|
|
}
|