Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/PhenikaaX/CiA402Helper.cs

@@ -0,0 +1,165 @@
+namespace RobotNet10.RobotApp.Drivers.PhenikaaX;
+
+public static class CiA402Helper
+{
+
+    /// <summary>
+    /// Extract value từ PDO data theo bit offset và bit length
+    /// </summary>
+    public static byte[] ExtractValueFromPdoData(byte[] pdoData, int bitOffset, byte bitLength)
+    {
+        int byteOffset = bitOffset / 8;
+        int bitOffsetInByte = bitOffset % 8;
+        int byteLength = (bitLength + 7) / 8; // Round up
+
+        if (byteOffset + byteLength > pdoData.Length)
+        {
+            throw new ArgumentException($"PDO data too short for extraction at bit offset {bitOffset}, length {bitLength}");
+        }
+
+        byte[] result = new byte[byteLength];
+
+        if (bitOffsetInByte == 0 && bitLength % 8 == 0)
+        {
+            // Aligned extraction - simple copy
+            Array.Copy(pdoData, byteOffset, result, 0, byteLength);
+        }
+        else
+        {
+            // Unaligned extraction - need bit manipulation
+            ulong value = 0;
+            int bitsRead = 0;
+            int currentByteOffset = byteOffset;
+            int currentBitOffset = bitOffsetInByte;
+
+            while (bitsRead < bitLength && currentByteOffset < pdoData.Length)
+            {
+                int bitsToRead = Math.Min(8 - currentBitOffset, bitLength - bitsRead);
+                byte mask = (byte)((1 << bitsToRead) - 1);
+                byte byteValue = (byte)((pdoData[currentByteOffset] >> currentBitOffset) & mask);
+
+                value |= (ulong)byteValue << bitsRead;
+
+                bitsRead += bitsToRead;
+                currentByteOffset++;
+                currentBitOffset = 0;
+            }
+
+            // Convert ulong to byte array
+            for (int i = 0; i < byteLength; i++)
+            {
+                result[i] = (byte)(value >> (i * 8));
+            }
+        }
+
+        return result;
+    }
+
+
+    /// <summary>
+    /// Parse object index từ string (hỗ trợ hex format: 0x6040 hoặc decimal: 24640)
+    /// </summary>
+    public static bool TryParseObjectIndex(string indexStr, out ushort index)
+    {
+        index = 0;
+        if (string.IsNullOrWhiteSpace(indexStr))
+            return false;
+
+        indexStr = indexStr.Trim();
+        if (indexStr.StartsWith("0x", StringComparison.OrdinalIgnoreCase))
+        {
+            return ushort.TryParse(indexStr.AsSpan(2), System.Globalization.NumberStyles.HexNumber, null, out index);
+        }
+        else
+        {
+            return ushort.TryParse(indexStr, out index);
+        }
+    }
+
+    /// <summary>
+    /// Convert bytes to value type với sign extension nếu cần
+    /// </summary>
+    public static T ConvertBytesToValue<T>(byte[] bytes, byte bitLength) where T : struct
+    {
+        ulong value = 0;
+        for (int i = 0; i < Math.Min(bytes.Length, 8); i++)
+        {
+            value |= (ulong)bytes[i] << (i * 8);
+        }
+
+        var type = typeof(T);
+        if (type == typeof(ushort))
+            return (T)(object)(ushort)value;
+        if (type == typeof(short))
+        {
+            if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFFFUL << bitLength;
+            return (T)(object)(short)value;
+        }
+        if (type == typeof(uint))
+            return (T)(object)(uint)value;
+        if (type == typeof(int))
+        {
+            if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFFFFFFFUL << bitLength;
+            return (T)(object)(int)value;
+        }
+        if (type == typeof(byte))
+            return (T)(object)(byte)value;
+        if (type == typeof(sbyte))
+        {
+            if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
+                value |= 0xFFUL << bitLength;
+            return (T)(object)(sbyte)value;
+        }
+
+        throw new NotSupportedException($"Type {type.Name} is not supported");
+    }
+
+    /// <summary>
+    /// Convert value to bytes theo bitLength được map trong PDO
+    /// Mục đích của bitLength:
+    /// 1. Truncate/mask giá trị nếu vượt quá số bits được map
+    /// 2. Đảm bảo chỉ gửi đúng số bytes cần thiết
+    /// 3. Xử lý các trường hợp partial mapping (ví dụ: map 8 bits của ushort 16 bits)
+    /// </summary>
+    public static byte[] ConvertValueToBytes<T>(T value, byte bitLength) where T : struct
+    {
+        var type = typeof(T);
+        int byteLength = (bitLength + 7) / 8; // Round up to nearest byte
+
+        // Convert value to ulong để xử lý mask
+        ulong rawValue = 0;
+        if (type == typeof(ushort))
+            rawValue = (ushort)(object)value!;
+        else if (type == typeof(short))
+            rawValue = (ulong)(ushort)(short)(object)value!; // Convert signed to unsigned
+        else if (type == typeof(uint))
+            rawValue = (uint)(object)value!;
+        else if (type == typeof(int))
+            rawValue = (ulong)(uint)(int)(object)value!; // Convert signed to unsigned
+        else if (type == typeof(byte))
+            rawValue = (byte)(object)value!;
+        else if (type == typeof(sbyte))
+            rawValue = (ulong)(byte)(sbyte)(object)value!;
+        else
+            throw new NotSupportedException($"Type {type.Name} is not supported");
+
+        // Mask để chỉ lấy số bits được map
+        if (bitLength < 64)
+        {
+            ulong mask = (1UL << bitLength) - 1;
+            rawValue &= mask;
+        }
+
+        // Convert to byte array với đúng số bytes
+        byte[] result = new byte[byteLength];
+        for (int i = 0; i < byteLength && i < 8; i++)
+        {
+            result[i] = (byte)(rawValue >> (i * 8));
+        }
+
+        return result;
+    }
+
+}