From ce6b6f6c6f18e12b782c4c0e2ed2e62cfef9e0a0 Mon Sep 17 00:00:00 2001
From: lhiven <lhiven@163.com>
Date: Fri, 11 Oct 2024 13:45:13 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BC=98=E5=8C=96=E5=9B=BA=E9=AB=98=E7=9A=84?=
 =?UTF-8?q?=E4=BD=8D=E7=BD=AE=E6=AF=94=E8=BE=83=EF=BC=8C=E6=94=B9=E7=94=A8?=
 =?UTF-8?q?=E4=BA=8C=E7=BB=B4=E6=AF=94=E8=BE=83=E7=9A=84=E4=B8=80=E7=BB=B4?=
 =?UTF-8?q?=E6=A8=A1=E5=BC=8F?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Rs.Motion/GugaoPulse/GugaoPulseAxis.cs | 117 ++++++++++++++++++++-----
 1 file changed, 96 insertions(+), 21 deletions(-)

diff --git a/Rs.Motion/GugaoPulse/GugaoPulseAxis.cs b/Rs.Motion/GugaoPulse/GugaoPulseAxis.cs
index 5a4b307..f4f27a3 100644
--- a/Rs.Motion/GugaoPulse/GugaoPulseAxis.cs
+++ b/Rs.Motion/GugaoPulse/GugaoPulseAxis.cs
@@ -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);
-            if (apiResult != 0)
+            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)
             {
-                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);
+                LogHelper.Debug($"{apiResult} = mc_pulse.GT_2DCompareSetPrm({(short)Config.CardId}, {(short)(channel - 1)}, ref prm);");
+                return ErrorCode.Fail;
             }
-            else if(channel==2)
+            apiResult = mc_pulse.GT_2DCompareMode((short)Config.CardId, (short)(channel - 1), mc_pulse.COMPARE2D_MODE_1D);
+            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_2DCompareMode({(short)Config.CardId}, {(short)(channel - 1)}, mc_pulse.COMPARE2D_MODE_1D);");
+                return ErrorCode.Fail;
             }
-            if (apiResult != 0)
+            //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++)
             {
+                GetEncoderPosition(out double curPos);
+                double offset = postions[i] - curPos;
+                MmToPulse(offset, out double pulse);
+                compareData[i].px = 0;
+                compareData[i].py = (int)pulse;
+            }
+            apiResult = mc_pulse.GT_2DCompareData((short)Config.CardId, (short)(channel - 1), (short)postions.Length, ref compareData[0], 0);
+            if(apiResult!=0)
+            {
+                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)
+            if (value != 0)
             {
                 value = (short)(1 << (channel - 1));
-                
+
             }
-            apiResult = mc_pulse.GT_ComparePulse((short)Config.CardId, (short)(1<<(channel-1)), (short)(dp?1:0), (short)Config.HcmpPulseWidth);
-            if(apiResult != 0)
+            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)
             //{