QuanDT/SickApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

save

Browse Source
This commit is contained in:
DTQuan1212
2026-02-02 10:00:26 +07:00
commit b9b2c6ef79
617 changed files with 133854 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

123
SickBlazorApp/Services/EdsParser.cs Normal file
View File

@@ -0,0 +1,123 @@
using System;
using System.Collections.Generic;
using System.IO;
using System.Reflection;
namespace SickBlazorApp.Services
{
public class EdsParser
{
private readonly Dictionary<string, Dictionary<string, string>> _sections = new();
public EdsParser(string edsPath)
{
LoadEds(edsPath);
}
private void LoadEds(string edsPath)
{
// Đọc embed resource
var assembly = Assembly.GetExecutingAssembly();
using var stream = assembly.GetManifestResourceStream(edsPath);
if (stream == null) throw new FileNotFoundException("EDS not found: " + edsPath);
using var reader = new StreamReader(stream);
string? line;
string currentSection = string.Empty;
var currentSub = new Dictionary<string, string>();
while ((line = reader.ReadLine()) != null)
{
line = line.Trim();
if (string.IsNullOrEmpty(line) || line.StartsWith(";")) continue;
if (line.StartsWith("[") && line.EndsWith("]"))
{
if (!string.IsNullOrEmpty(currentSection))
{
_sections[currentSection] = currentSub;
}
currentSection = line[1..^1].ToLower(); // e.g. "1000" hoặc "1800sub1"
currentSub = new Dictionary<string, string>();
}
else if (line.Contains("="))
{
var parts = line.Split('=', 2);
var key = parts[0].Trim().ToLower();
var value = parts[1].Trim();
currentSub[key] = value;
}
}
if (!string.IsNullOrEmpty(currentSection))
{
_sections[currentSection] = currentSub;
}
}
// Helper lấy giá trị
public string GetValue(string section, string key)
{
section = section.ToLower();
if (_sections.TryGetValue(section, out var sub) && sub.TryGetValue(key.ToLower(), out var value))
{
return value;
}
return string.Empty;
}
// Ví dụ lấy TPDO1 COB-ID base (trả $nodeid + 0x180 → parse 0x180)
public uint GetTpdo1BaseCobId()
{
var cob = GetValue("1800sub1", "defaultvalue"); // "$NodeID + 0x180"
var hexPart = cob.Split('+')[1].Trim(); // "0x180"
return Convert.ToUInt32(hexPart, 16); // 384 (0x180)
}
// Lấy access-code (EDS không default, nhưng manual 0x98127634 → hard-code tạm hoặc thêm param)
public uint GetAccessCode()
{
// EDS không có default cho 0x2009sub1 → dùng từ manual
return 0x98127634; // Cập nhật từ EDS/manual
}
// Tính scale (mm per count)
public double GetPositionScale()
{
// Lấy resolution per revolution từ EDS (0x6001 default = 0x4000 = 16384 counts/rev)
var resPerRevHex = GetValue("6001", "defaultvalue") ?? "0x4000";
var countsPerRev = Convert.ToUInt32(resPerRevHex, 16); // 16384
// Thông số dây kéo BCG08: 230 mm per revolution (từ datasheet SICK BCG08 series)
const double mmPerRev = 230.0; // chuẩn từ datasheet wire draw
// Scale = mm per count = mm/rev / counts/rev
double scale = mmPerRev / countsPerRev;
// Kết quả ≈ 230 / 16384 ≈ 0.014038 mm/count
return scale;
}
// Lấy baudrate map (index → bitrate)
public Dictionary<byte, int> GetBaudrateMap()
{
var map = new Dictionary<byte, int>();
byte index = 0;
if (GetValue("deviceinfo", "baudrate_1000") == "1") map[index++] = 1000000;
if (GetValue("deviceinfo", "baudrate_800") == "1") map[index++] = 800000;
if (GetValue("deviceinfo", "baudrate_500") == "1") map[index++] = 500000;
if (GetValue("deviceinfo", "baudrate_250") == "1") map[index++] = 250000;
if (GetValue("deviceinfo", "baudrate_125") == "1") map[index++] = 125000;
if (GetValue("deviceinfo", "baudrate_100") == "1") map[index++] = 100000;
if (GetValue("deviceinfo", "baudrate_50") == "1") map[index++] = 50000;
if (GetValue("deviceinfo", "baudrate_20") == "1") map[index++] = 20000;
if (GetValue("deviceinfo", "baudrate_10") == "1") map[index++] = 10000;
return map;
}
// Kiểm tra position mapping (byte 0-3 uint32?)
public bool IsPositionMappedToTpdo1()
{
var mapping = GetValue("1a00sub1", "defaultvalue"); // 0x60040020
return mapping == "0x60040020"; // Position 32-bit
}
}
}

27
SickBlazorApp/Services/ICanBusService.cs Normal file
View File

@@ -0,0 +1,27 @@
using SickBlazorApp.Models;
namespace SickBlazorApp.Services;
public interface ICanBusService : IDisposable
{
event EventHandler<PositionPdo>? PositionReceived;
event EventHandler<CanNodeState>? NodeStateChanged;
event EventHandler<CanFrame>? FrameReceived;
event EventHandler<byte>? NodeIdChanged;
Task InitAsync();
void Start();
void Stop();
void SendNmtStart(byte nodeId);
void SendNmtReset(byte nodeId);
void SendNmtStop(byte nodeId);
void ChangeBitrate(int bitrate);
Task ApplyBitrateAsync(byte nodeId, int bitrate);
int CurrentBitrate { get; }
Task ApplyNodeIdAsync(byte oldNodeId, byte newNodeId);
byte CurrentNodeId { get; }
}

456
SickBlazorApp/Services/Windows/PcanCanService.cs Normal file
View File

@@ -0,0 +1,456 @@
using Microsoft.Extensions.Options;
using Peak.Can.Basic.BackwardCompatibility;
using SickBlazorApp.Models;
using SickBlazorApp.Options;
using System;
using System.Collections.Generic;
using System.IO;
using System.Reflection;
using System.Threading;
using System.Threading.Tasks;
namespace SickBlazorApp.Services.Windows;
public sealed class PcanCanService : ICanBusService
{
private readonly CanBusOptions _options;
private readonly EdsParser _edsParser; // Parser EDS mới
private CancellationTokenSource? _cts;
private bool _initialized;
public event EventHandler<PositionPdo>? PositionReceived;
private DateTime _lastHeartbeat = DateTime.MinValue;
private readonly TimeSpan _heartbeatTimeout = TimeSpan.FromSeconds(2);
public event EventHandler<CanNodeState>? NodeStateChanged;
private DateTime _lastSeen = DateTime.MinValue;
private CanNodeState _currentState = CanNodeState.Unknown;
public event EventHandler<CanFrame>? FrameReceived;
private int _currentBitrate;
public int CurrentBitrate { get; private set; }
private bool _waitingStopAck = false;
private bool _autoResumeEnabled = true;
private DateTime _lastBootup = DateTime.MinValue;
private byte _currentNodeId;
public byte CurrentNodeId => _currentNodeId;
public byte GetCurrentNodeId() => _currentNodeId;
public event EventHandler<byte>? NodeIdChanged;
private Dictionary<byte, int> _baudMap = new(); // Map baudrate động từ EDS
public PcanCanService(IOptions<CanBusOptions> options)
{
_options = options.Value;
_edsParser = new EdsParser("SickBlazorApp.eds.AHM36_I_CO.eds"); // hard-code tên resource
_options.PositionBaseCobId = _edsParser.GetTpdo1BaseCobId(); // vẫn động
_options.PositionScale = _edsParser.GetPositionScale(); // động từ 230 mm/rev
if (!_edsParser.IsPositionMappedToTpdo1())
throw new InvalidOperationException("Position mapping sai");
_baudMap = _edsParser.GetBaudrateMap();
CurrentBitrate = _currentBitrate;
}
// ================= INIT =================
public Task InitAsync()
{
ushort channel = ParseChannel(_options.Channel);
if (_initialized)
return Task.CompletedTask;
var baud = MapBaudrate(_currentBitrate);
var status = PCANBasic.Initialize(channel, baud);
if (status != TPCANStatus.PCAN_ERROR_OK)
throw new InvalidOperationException($"PCAN init failed: {status}");
_initialized = true;
CurrentBitrate = _currentBitrate;
return Task.CompletedTask;
}
// ================= START READ =================
public void Start()
{
if (_cts != null)
return;
_cts = new CancellationTokenSource();
ushort channel = ParseChannel(_options.Channel);
uint pdoCobId = GetPositionCobId();
uint heartbeatCobId = 0x700u + _currentNodeId;
double scale = _options.PositionScale;
Task.Run(() =>
{
while (!_cts!.IsCancellationRequested)
{
TPCANMsg msg;
TPCANTimestamp ts;
var result = PCANBasic.Read(channel, out msg, out ts);
// ================= BOOT-UP DETECT =================
if (msg.ID >= 0x701 && msg.ID <= 0x77F && msg.LEN >= 1 && msg.DATA[0] == 0x00)
{
byte detectedNodeId = (byte)(msg.ID - 0x700);
if (_currentNodeId != detectedNodeId)
{
_currentNodeId = detectedNodeId;
NodeIdChanged?.Invoke(this, detectedNodeId);
}
}
if (result != TPCANStatus.PCAN_ERROR_OK)
{
CheckHeartbeatTimeout();
Thread.Sleep(5);
continue;
}
string dataHex = string.Join(" ", msg.DATA.Take(msg.LEN).Select(b => b.ToString("X2")));
FrameReceived?.Invoke(this, new CanFrame
{
CobId = msg.ID,
Length = msg.LEN,
DataHex = dataHex,
Timestamp = DateTime.Now
});
if (!msg.MSGTYPE.HasFlag(TPCANMessageType.PCAN_MESSAGE_STANDARD))
continue;
// ================= HEARTBEAT =================
if (msg.ID >= 0x701 && msg.ID <= 0x77F && msg.LEN >= 1)
{
_lastHeartbeat = DateTime.Now;
_lastSeen = DateTime.Now;
byte nodeIdFromHb = (byte)(msg.ID - 0x700);
byte hbValue = msg.DATA[0];
if (_currentNodeId != nodeIdFromHb)
{
_currentNodeId = nodeIdFromHb;
NodeIdChanged?.Invoke(this, nodeIdFromHb);
}
var hbState = hbValue switch
{
0x00 => CanNodeState.Bootup,
0x04 => CanNodeState.Stopped,
0x05 => CanNodeState.Operational,
0x7F => CanNodeState.PreOperational,
_ => CanNodeState.Unknown
};
UpdateState(hbState);
if (_autoResumeEnabled && !_waitingStopAck && hbState == CanNodeState.Bootup)
{
if (DateTime.Now - _lastBootup > TimeSpan.FromSeconds(1))
{
_lastBootup = DateTime.Now;
Task.Run(async () =>
{
await Task.Delay(500);
SendNmtStart(_currentNodeId);
});
}
}
if (_waitingStopAck && hbState == CanNodeState.Stopped)
{
_waitingStopAck = false;
}
continue;
}
// ================= POSITION PDO =================
if (msg.ID < 0x180 || msg.ID > 0x1FF || msg.LEN < 6)
continue;
_lastSeen = DateTime.Now;
uint raw = (uint)(
msg.DATA[0] |
(msg.DATA[1] << 8) |
(msg.DATA[2] << 16) |
(msg.DATA[3] << 24)
);
double positionMm = raw * scale;
positionMm = Math.Min(positionMm, 3000.0);
positionMm = Math.Max(positionMm, 0.0);
PositionReceived?.Invoke(this, new PositionPdo
{
CobId = msg.ID,
Length = msg.LEN,
DataHex = dataHex,
RawValue = raw,
PositionMm = positionMm,
Timestamp = DateTime.Now
});
FrameReceived?.Invoke(this, new CanFrame
{
CobId = msg.ID,
Length = msg.LEN,
DataHex = dataHex,
Timestamp = DateTime.Now,
PositionMm = positionMm,
Description = "TPDO1 Position"
});
}
});
}
// ================= STOP =================
public void Stop()
{
if (_cts == null)
return;
_waitingStopAck = true;
SendNmtStop(_currentNodeId);
_cts.Cancel();
_cts = null;
UpdateState(CanNodeState.Stopped);
}
public void Dispose() => Stop();
// ================= NMT =================
public void SendNmtStart(byte nodeId)
{
ushort channel = ParseChannel(_options.Channel);
var msg = new TPCANMsg
{
ID = 0x000,
LEN = 2,
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
DATA = new byte[8]
};
msg.DATA[0] = 0x01; // NMT Start
msg.DATA[1] = nodeId;
for (int i = 0; i < 3; i++)
{
PCANBasic.Write(channel, ref msg);
Thread.Sleep(50);
}
}
public void SendNmtStop(byte nodeId)
{
ushort channel = ParseChannel(_options.Channel);
var msg = new TPCANMsg
{
ID = 0x000,
LEN = 2,
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
DATA = new byte[8]
};
msg.DATA[0] = 0x02; // NMT Stop
msg.DATA[1] = nodeId;
PCANBasic.Write(channel, ref msg);
}
public void SendNmtReset(byte nodeId)
{
ushort channel = ParseChannel(_options.Channel);
var msg = new TPCANMsg
{
ID = 0x000,
LEN = 2,
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
DATA = new byte[8]
};
msg.DATA[0] = 0x81; // Reset node
msg.DATA[1] = nodeId;
PCANBasic.Write(channel, ref msg);
}
// ================= HELPERS =================
private static ushort ParseChannel(string ch) => ch switch
{
"PCAN_USBBUS1" => 0x51,
"PCAN_USBBUS2" => 0x52,
_ => throw new ArgumentOutOfRangeException(nameof(ch))
};
private static TPCANBaudrate MapBaudrate(int b) => b switch
{
1000000 => TPCANBaudrate.PCAN_BAUD_1M,
800000 => TPCANBaudrate.PCAN_BAUD_800K,
500000 => TPCANBaudrate.PCAN_BAUD_500K,
250000 => TPCANBaudrate.PCAN_BAUD_250K,
125000 => TPCANBaudrate.PCAN_BAUD_125K,
100000 => TPCANBaudrate.PCAN_BAUD_100K,
50000 => TPCANBaudrate.PCAN_BAUD_50K,
20000 => TPCANBaudrate.PCAN_BAUD_20K,
_ => throw new ArgumentOutOfRangeException(nameof(b))
};
private uint GetPositionCobId()
{
return _options.PositionBaseCobId + _currentNodeId;
}
private void CheckHeartbeatTimeout()
{
if (_currentState == CanNodeState.PreOperational || _currentState == CanNodeState.Stopped)
return;
if (_lastHeartbeat == DateTime.MinValue)
return;
if (DateTime.Now - _lastHeartbeat > _heartbeatTimeout)
{
UpdateState(CanNodeState.Timeout);
}
}
private void UpdateState(CanNodeState newState)
{
if (_currentState == newState)
return;
_currentState = newState;
NodeStateChanged?.Invoke(this, newState);
}
public void ChangeBitrate(int bitrate)
{
if (_currentBitrate == bitrate)
return;
ushort channel = ParseChannel(_options.Channel);
Stop();
if (_initialized)
{
PCANBasic.Uninitialize(channel);
_initialized = false;
}
_currentBitrate = bitrate;
}
private void SendRaw(uint cobId, byte[] data)
{
ushort channel = ParseChannel(_options.Channel);
var msg = new TPCANMsg
{
ID = cobId,
LEN = (byte)data.Length,
MSGTYPE = TPCANMessageType.PCAN_MESSAGE_STANDARD,
DATA = new byte[8]
};
Array.Copy(data, msg.DATA, data.Length);
PCANBasic.Write(channel, ref msg);
}
private uint GetAccessCode() => _edsParser.GetAccessCode(); // 0x98127634
private byte GetBitrateIndex(int bitrate)
{
foreach (var kv in _baudMap)
{
if (kv.Value == bitrate) return kv.Key;
}
throw new ArgumentOutOfRangeException(nameof(bitrate), $"Bitrate {bitrate} not supported in EDS");
}
public async Task ApplyBitrateAsync(byte nodeId, int bitrate)
{
// Pre-Operational
SendRaw(0x000, new byte[] { 0x80, nodeId });
await Task.Delay(100);
// Unlock access code (đọc động từ EDS/manual)
uint accessCode = GetAccessCode();
SendRaw(
(uint)(0x600 + nodeId),
new byte[] { 0x23, 0x09, 0x20, 0x01,
(byte)(accessCode & 0xFF),
(byte)((accessCode >> 8) & 0xFF),
(byte)((accessCode >> 16) & 0xFF),
(byte)((accessCode >> 24) & 0xFF) }
);
await Task.Delay(100);
// Set bitrate (index từ EDS map)
byte bitrateValue = GetBitrateIndex(bitrate);
SendRaw(
(uint)(0x600 + nodeId),
new byte[] { 0x2F, 0x09, 0x20, 0x03, bitrateValue, 0x00, 0x00, 0x00 }
);
await Task.Delay(100);
// Save EEPROM
SendRaw(
(uint)(0x600 + nodeId),
new byte[] { 0x23, 0x10, 0x10, 0x01, 0x73, 0x61, 0x76, 0x65 }
);
await Task.Delay(100);
// Reset node
SendRaw(0x000u, new byte[] { 0x81, nodeId });
// Update runtime
await Task.Delay(1200);
ChangeBitrate(bitrate);
CurrentBitrate = bitrate;
}
public async Task ApplyNodeIdAsync(byte oldNodeId, byte newNodeId)
{
if (newNodeId < 1 || newNodeId > 127)
throw new ArgumentOutOfRangeException(nameof(newNodeId));
// Pre-Operational
SendRaw(0x000, new byte[] { 0x80, oldNodeId });
await Task.Delay(100);
// Unlock access code (động từ EDS)
uint accessCode = GetAccessCode();
SendRaw(
(uint)(0x600 + oldNodeId),
new byte[] { 0x23, 0x09, 0x20, 0x01,
(byte)(accessCode & 0xFF),
(byte)((accessCode >> 8) & 0xFF),
(byte)((accessCode >> 16) & 0xFF),
(byte)((accessCode >> 24) & 0xFF) }
);
await Task.Delay(100);
// Set Node ID
SendRaw(
(uint)(0x600 + oldNodeId),
new byte[] { 0x2F, 0x09, 0x20, 0x02, newNodeId, 0x00, 0x00, 0x00 }
);
await Task.Delay(100);
// Save EEPROM
SendRaw(
(uint)(0x600 + oldNodeId),
new byte[] { 0x23, 0x10, 0x10, 0x01, 0x73, 0x61, 0x76, 0x65 }
);
await Task.Delay(100);
// Reset node
SendRaw(0x000, new byte[] { 0x81, oldNodeId });
// Thông báo UI
_currentNodeId = newNodeId;
NodeIdChanged?.Invoke(this, newNodeId);
}
}