lhiven
/
RealSys.MotionPlat
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

优化固高的位置比较，改用二维比较的一维模式

Browse Source
master
lhiven 6 months ago
parent de4d7aad31
commit ce6b6f6c6f
1 changed files with 96 additions and 21 deletions
Show Stats Download Patch File Download Diff File

105
Rs.Motion/GugaoPulse/GugaoPulseAxis.cs
Unescape Escape View File

@ -11,6 +11,7 @@ using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using static gts.mc_pulse;
namespace Rs.Motion.GugaoPulse
{
@ -1626,32 +1627,100 @@ namespace Rs.Motion.GugaoPulse
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++)
apiResult = mc_pulse.GT_2DCompareStop((short)Config.CardId, (short)(channel - 1));
if(apiResult!=0)
{
GetEncoderPosition(out double curPos);
double offset = postions[i] - curPos;
MmToPulse(offset, out double pulse);
compareDatas[i] = (int)pulse;
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareStop({(short)Config.CardId}, {(short)(channel - 1)});");
return ErrorCode.Fail;
}
apiResult = mc_pulse.GT_CompareStop((short)Config.CardId);
apiResult = mc_pulse.GT_2DCompareMode((short)Config.CardId, (short)(channel - 1), mc_pulse.COMPARE2D_MODE_1D);
if(apiResult!=0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareMode({(short)Config.CardId}, {(short)(channel - 1)}, mc_pulse.COMPARE2D_MODE_1D);");
return ErrorCode.Fail;
}
if(channel==1)
T2DCompareData[] compareData = new T2DCompareData[postions.Length];
T2DComparePrm prm;
prm.encx = 1;
prm.ency = (short)Config.AxisId;
prm.maxerr = 3;
prm.outputType = 0;
prm.source = 1;
prm.startLevel = 0;
prm.threshold = 0;
prm.time = (short)Config.HcmpPulseWidth;
apiResult = mc_pulse.GT_2DCompareSetPrm((short)Config.CardId, (short)(channel - 1), ref prm);
if(apiResult!=0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareSetPrm({(short)Config.CardId}, {(short)(channel - 1)}, ref prm);");
return ErrorCode.Fail;
}
apiResult = mc_pulse.GT_2DCompareMode((short)Config.CardId, (short)(channel - 1), mc_pulse.COMPARE2D_MODE_1D);
if(apiResult!=0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareMode({(short)Config.CardId}, {(short)(channel - 1)}, mc_pulse.COMPARE2D_MODE_1D);");
return ErrorCode.Fail;
}
//int[] compareDatas1 = new int[postions.Length];
//int[] compareDatas2 = new int[postions.Length];
//if(channel==1)
//{
// for (int i = 0; i < postions.Length; i++)
// {
// GetEncoderPosition(out double curPos);
// double offset = postions[i] - curPos;
// MmToPulse(offset, out double pulse);
// compareDatas1[i] = (int)pulse;
// }
//}
//else if(channel==2)
//{
// for (int i = 0; i < postions.Length; i++)
// {
// GetEncoderPosition(out double curPos);
// double offset = postions[i] - curPos;
// MmToPulse(offset, out double pulse);
// compareDatas2[i] = (int)pulse;
// }
//}
for (int i = 0; i < postions.Length; i++)
{
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);
GetEncoderPosition(out double curPos);
double offset = postions[i] - curPos;
MmToPulse(offset, out double pulse);
compareData[i].px = 0;
compareData[i].py = (int)pulse;
}
else if(channel==2)
apiResult = mc_pulse.GT_2DCompareData((short)Config.CardId, (short)(channel - 1), (short)postions.Length, ref compareData[0], 0);
if(apiResult!=0)
{
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);
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareData({(short)Config.CardId}, {(short)(channel - 1)}, {(short)postions.Length}, ref compareData[0], 0);");
return ErrorCode.Fail;
}
apiResult = mc_pulse.GT_2DCompareStart((short)Config.CardId, (short)(channel - 1));
if(apiResult!=0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareStart({(short)Config.CardId}, {(short)(channel - 1)});");
return ErrorCode.Fail;
}
//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 compareDatas1[0], (short)compareDatas1.Length, ref compareDatas2[0], 0);
//}
//else if(channel==2)
//{
// apiResult = mc_pulse.GT_CompareData((short)Config.CardId, (short)Config.AxisId, 1, 0, 0, (short)Config.HcmpPulseWidth, ref compareDatas1[0],0, ref compareDatas2[0], (short)compareDatas2.Length);
//}
//if (apiResult != 0)
//{
// return ErrorCode.Fail;
//}
return ErrorCode.Ok;
}
@ -1670,15 +1739,19 @@ namespace Rs.Motion.GugaoPulse
public override ErrorCode ComparePulse(ushort channel,bool dp=true)
{
//apiResult = mc_pulse.GT_2DComparePulse((short)Config.CardId, (short)(channel - 1), 1, 0, (short)Config.HcmpPulseWidth);
short value = (short)channel;
if (value != 0)
{
value = (short)(1 << (channel - 1));
}
ClearCompare(channel);
apiResult = mc_pulse.GT_ComparePulse((short)Config.CardId, (short)(1 << (channel - 1)), (short)(dp ? 1 : 0), (short)Config.HcmpPulseWidth);
if (apiResult != 0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_ComparePulse({(short)Config.CardId}, {(short)(1 << (channel - 1))}, {(short)(dp ? 1 : 0)}, {(short)Config.HcmpPulseWidth});");
return ErrorCode.Fail;
}
return ErrorCode.Ok;
@ -1686,9 +1759,11 @@ namespace Rs.Motion.GugaoPulse
public override ErrorCode ClearCompare(ushort uChannel)
{
apiResult = mc_pulse.GT_CompareStop((short)Config.CardId);
apiResult = mc_pulse.GT_2DCompareStop((short)Config.CardId,(short)(uChannel-1));
//apiResult = mc_pulse.GT_CompareStop((short)Config.CardId);
if (apiResult != 0)
{
LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareStop({(short)Config.CardId},{(short)(uChannel-1)});");
return ErrorCode.Fail;
}
return ErrorCode.Ok;
@ -1714,8 +1789,8 @@ namespace Rs.Motion.GugaoPulse
oConfig.reverse = 0;
mc_pulse_cfg.GT_SetDoConfig((short)Config.CardId, mc_pulse.MC_CLEAR, (short)Config.AxisId, ref oConfig);
LogHelper.Debug($" mc_pulse_cfg.GT_SetDoConfig({(short)Config.CardId}, {mc_pulse.MC_CLEAR}, {(short)Config.AxisId}, ref oConfig);");
LogHelper.Debug($"active={oConfig.active},axis={oConfig.axis},axisitem={oConfig.axisItem},reverse={oConfig.reverse}");
//LogHelper.Debug($" mc_pulse_cfg.GT_SetDoConfig({(short)Config.CardId}, {mc_pulse.MC_CLEAR}, {(short)Config.AxisId}, ref oConfig);");
//LogHelper.Debug($"active={oConfig.active},axis={oConfig.axis},axisitem={oConfig.axisItem},reverse={oConfig.reverse}");
//apiResult = mc_pulse.GT_SetDoBit((short)Config.CardId, mc_pulse.MC_CLEAR, (short)Config.AxisId, 0);
//if (apiResult != 0)
//{

Write Preview
Loading…
Cancel
Save