489 lines
22 KiB
C#
489 lines
22 KiB
C#
using RobotNet10.CANOpen;
|
||
using RobotNet10.CANOpen.Interfaces;
|
||
using System.Collections.Concurrent;
|
||
|
||
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
|
||
/// <summary>
|
||
/// Charger Simulator cho VARTA EasyBlade 59V
|
||
/// Máy tính đóng vai Charger, giao tiếp với pin thật qua USB-CAN adapter.
|
||
///
|
||
/// ── Thông số cố định (theo Technical Spec V1.8) ──────────────────────
|
||
/// Baud rate : 250 kbit/s
|
||
/// Charger Node ID : 100 (0x64)
|
||
/// Max voltage : 58.8 V (4 × 12V × 1.225 cells)
|
||
/// Max current : 25 A
|
||
/// Heartbeat : mỗi 1000 ms → COB-ID 0x764
|
||
/// RPDO1 : mỗi 200 ms → COB-ID 0x1E4
|
||
/// ─────────────────────────────────────────────────────────────────────
|
||
/// </summary>
|
||
public class Charger59V
|
||
{
|
||
// ════════════════════════════════════════════════════════════════
|
||
// ⚙️ THÔNG SỐ HARDCODE – CHỈNH TẠI ĐÂY NẾU CẦN
|
||
// ════════════════════════════════════════════════════════════════
|
||
|
||
// Điện áp tối đa charger có thể cung cấp (V)
|
||
// EasyBlade 59V: pin lithium 13S → max 54.6V, để an toàn dùng 58.8V
|
||
private const double MAX_VOLTAGE_V = 58.8;
|
||
|
||
// Dòng tối đa charger có thể cung cấp (A)
|
||
private const double MAX_CURRENT_A = 25.0;
|
||
|
||
// Điện áp thực đo được (báo lại pin trong RPDO1, byte 2-3)
|
||
// Lúc chưa sạc thực thì đặt bằng Max hoặc giá trị đo thực của nguồn
|
||
private const double ACTUAL_VOLTAGE_V = 54.0;
|
||
|
||
// Dòng thực đo được (báo lại pin trong RPDO1, byte 0-1)
|
||
private const double ACTUAL_CURRENT_A = 10.0;
|
||
|
||
// COB-ID (không đổi theo spec)
|
||
private const uint COB_HEARTBEAT = 0x764; // gửi
|
||
private const uint COB_RPDO1 = 0x1E4; // gửi
|
||
private const uint COB_SDO_TX = 0x5E4; // gửi (response về battery)
|
||
private const uint COB_SDO_RX = 0x664; // nhận (request từ battery)
|
||
private const uint COB_TPDO9 = 0x264; // nhận (SoC, VReq, IReq)
|
||
private const uint COB_TPDO8 = 0x49B; // nhận (charge control status)
|
||
|
||
// ── Giá trị raw (Q8 = ×256, Q4 = ×16) ───────────────────────────
|
||
private static readonly ushort RAW_MAX_VOLTAGE = (ushort)(MAX_VOLTAGE_V * 256);
|
||
private static readonly ushort RAW_MAX_CURRENT = (ushort)(MAX_CURRENT_A * 16);
|
||
private static readonly ushort RAW_ACT_VOLTAGE = (ushort)(ACTUAL_VOLTAGE_V * 256);
|
||
private static readonly ushort RAW_ACT_CURRENT = (ushort)(ACTUAL_CURRENT_A * 256);
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// State
|
||
// ════════════════════════════════════════════════════════════════
|
||
private bool _sdoInitDone = false;
|
||
private bool _chargeActive = false; // true sau khi set Bit12
|
||
private bool _relayOpen = true; // true = relay mở, không có điện ra
|
||
private bool _batteryCharging = false; // true khi pin báo đang vào trạng thái sạc
|
||
|
||
// Lưu lại giá trị SDO battery ghi vào charger
|
||
private byte _batteryStatus = 0; // Object 0x6000
|
||
private byte _chargeControl = 0; // Object 0x4200
|
||
private ushort _voltageReqRaw = 0; // Object 0x2276
|
||
private ushort _currentReqRaw = 0; // Object 0x6070
|
||
|
||
private readonly ICanOpenManager _canOpenManager;
|
||
private readonly string _canInterface;
|
||
private ICanBus? _can;
|
||
private readonly CancellationToken _ct;
|
||
private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
|
||
private readonly SemaphoreSlim _rxSignal = new(0);
|
||
|
||
public Charger59V(ICanOpenManager canOpenManager, string canInterface, CancellationToken ct)
|
||
{
|
||
_canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
|
||
_canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
|
||
_ct = ct;
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
public async Task RunAsync()
|
||
{
|
||
_can = await _canOpenManager.GetOrCreateCanBusAsync(_canInterface, _ct);
|
||
_can.FrameReceived += OnFrameReceived;
|
||
|
||
// Log($"Max Voltage : {MAX_VOLTAGE_V} V (raw Q8 = {RAW_MAX_VOLTAGE})");
|
||
// Log($"Max Current : {MAX_CURRENT_A} A (raw Q4 = {RAW_MAX_CURRENT})");
|
||
// Log($"Gửi Heartbeat 0x{COB_HEARTBEAT:X3} mỗi 1000ms...");
|
||
// Log("Đang chờ pin kết nối...\n");
|
||
|
||
try
|
||
{
|
||
// Chạy song song 3 vòng lặp
|
||
await Task.WhenAll(
|
||
HeartbeatLoopAsync(), // gửi HB mỗi 1000ms
|
||
ReceiveLoopAsync(), // nhận SDO + TPDO từ pin
|
||
Rpdo1LoopAsync() // gửi RPDO1 sau khi SDO init xong
|
||
);
|
||
}
|
||
finally
|
||
{
|
||
_can.FrameReceived -= OnFrameReceived;
|
||
while (_rxQueue.TryDequeue(out _)) { }
|
||
while (_rxSignal.Wait(0)) { }
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// VÒNG LẶP 1 – Heartbeat (mỗi 1000ms)
|
||
// ════════════════════════════════════════════════════════════════
|
||
private async Task HeartbeatLoopAsync()
|
||
{
|
||
while (!_ct.IsCancellationRequested)
|
||
{
|
||
// NMT Heartbeat: 1 byte [0x05] = Operational state
|
||
SendFrame(COB_HEARTBEAT, [0x05]);
|
||
// Dim($"♥ HB → 0x{COB_HEARTBEAT:X3}");
|
||
await Task.Delay(1000, _ct);
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// VÒNG LẶP 2 – Nhận frame từ pin
|
||
// ════════════════════════════════════════════════════════════════
|
||
private async Task ReceiveLoopAsync()
|
||
{
|
||
while (!_ct.IsCancellationRequested)
|
||
{
|
||
if (TryReceive(out var frame))
|
||
{
|
||
switch (frame.CanId)
|
||
{
|
||
case COB_SDO_RX: HandleSdo(frame.Data); break;
|
||
case COB_TPDO9: HandleTpdo9(frame.Data); break;
|
||
case COB_TPDO8: HandleTpdo8(frame.Data); break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
await Task.Delay(1, _ct); // yield CPU khi không có frame
|
||
}
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// VÒNG LẶP 3 – Gửi RPDO1 (mỗi 200ms, sau khi SDO init xong)
|
||
// ════════════════════════════════════════════════════════════════
|
||
private async Task Rpdo1LoopAsync()
|
||
{
|
||
// Chờ SDO init hoàn tất
|
||
while (!_sdoInitDone && !_ct.IsCancellationRequested)
|
||
await Task.Delay(50, _ct);
|
||
|
||
if (_ct.IsCancellationRequested) return;
|
||
|
||
// Delay 1s trước khi kích hoạt Bit12 (cho pin ổn định)
|
||
LogOk("SDO init xong! Chờ 1s rồi bật Bit12...");
|
||
await Task.Delay(1000, _ct);
|
||
|
||
// Bật relay và charge mode
|
||
_relayOpen = false;
|
||
_chargeActive = true;
|
||
LogOk("==> Bit12 SET – Pin đang chuyển sang CHARGE MODE!");
|
||
|
||
// Gửi RPDO1 mỗi 200ms
|
||
while (!_ct.IsCancellationRequested)
|
||
{
|
||
SendRpdo1();
|
||
await Task.Delay(200, _ct);
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// GỬI RPDO1 (COB-ID 0x1E4)
|
||
//
|
||
// Byte 0-1: Charging Current [1/256 A, Q8]
|
||
// Byte 2-3: Charging Voltage [1/256 V, Q8]
|
||
// Byte 4-5: Max avail Current [1/16 A, Q4]
|
||
// Byte 6-7: Extended Charger Status
|
||
// → Bit12 (0x1000) = kích hoạt charge mode
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void SendRpdo1()
|
||
{
|
||
ushort extStatus = (_chargeActive && !_relayOpen)
|
||
? (ushort)0x1000 // Bit12 set
|
||
: (ushort)0x0000;
|
||
|
||
byte[] data =
|
||
[
|
||
(byte)(RAW_ACT_CURRENT & 0xFF), (byte)(RAW_ACT_CURRENT >> 8), // Byte 0-1
|
||
(byte)(RAW_ACT_VOLTAGE & 0xFF), (byte)(RAW_ACT_VOLTAGE >> 8), // Byte 2-3
|
||
(byte)(RAW_MAX_CURRENT & 0xFF), (byte)(RAW_MAX_CURRENT >> 8), // Byte 4-5
|
||
(byte)(extStatus & 0xFF), (byte)(extStatus >> 8), // Byte 6-7
|
||
];
|
||
|
||
SendFrame(COB_RPDO1, data);
|
||
// Dim($"→ RPDO1 0x{COB_RPDO1:X3} [{string.Join(" ", data.Select(b => $"{b:X2}"))}] " +
|
||
// $"ExtStat=0x{extStatus:X4}");
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// XỬ LÝ SDO REQUEST TỪ PIN (COB-ID 0x664)
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void HandleSdo(byte[] d)
|
||
{
|
||
if (d.Length < 8) return;
|
||
byte cmd = d[0];
|
||
ushort index = (ushort)(d[1] | (d[2] << 8));
|
||
byte sub = d[3];
|
||
|
||
switch (cmd)
|
||
{
|
||
// Pin GHI vào object của charger
|
||
case 0x2F: // Write 1 byte
|
||
OnWrite(index, sub, d[4], 0);
|
||
break;
|
||
case 0x2B: // Write 2 bytes
|
||
OnWrite(index, sub, d[4], (ushort)(d[4] | (d[5] << 8)));
|
||
break;
|
||
case 0x23: // Write 4 bytes
|
||
SdoWriteOk(index, sub); // phản hồi OK, bỏ qua giá trị
|
||
break;
|
||
|
||
// Pin ĐỌC object từ charger
|
||
case 0x40: // Read request
|
||
OnRead(index, sub);
|
||
break;
|
||
}
|
||
}
|
||
|
||
private void OnWrite(ushort index, byte sub, byte val8, ushort val16)
|
||
{
|
||
switch (index)
|
||
{
|
||
case 0x6000: // Battery Status
|
||
_batteryStatus = val8;
|
||
// Log($" [SDO] 0x6000 Battery Status ← {val8} " +
|
||
// (val8 == 1 ? "→ Relay CLOSED (power ON)" : "→ Relay OPEN (power OFF)"));
|
||
_relayOpen = (val8 == 0);
|
||
break;
|
||
|
||
case 0x4200: // Charge Control
|
||
_chargeControl = val8;
|
||
// Log($" [SDO] 0x4200 Charge Control ← {val8} " +
|
||
// (val8 == 1 ? "→ Battery READY" : "→ Battery NOT ready"));
|
||
// ChargeControl=0 → pin báo full/lỗi → tắt relay
|
||
if (val8 == 0 && _sdoInitDone)
|
||
{
|
||
LogWarn("ChargeControl=0 → TẮT RELAY (pin đầy hoặc lỗi)");
|
||
_relayOpen = true;
|
||
_chargeActive = false;
|
||
}
|
||
break;
|
||
|
||
case 0x2276: // Voltage Request
|
||
_voltageReqRaw = val16;
|
||
// Log($" [SDO] 0x2276 Voltage Request ← {val16 / 256.0:F3} V");
|
||
break;
|
||
|
||
case 0x6070: // Current Request
|
||
_currentReqRaw = val16;
|
||
// Log($" [SDO] 0x6070 Current Request ← {val16 / 16.0:F3} A");
|
||
break;
|
||
|
||
default:
|
||
// Log($" [SDO] Write idx=0x{index:X4}.{sub} val=0x{val16:X4}");
|
||
break;
|
||
}
|
||
SdoWriteOk(index, sub);
|
||
CheckInitComplete();
|
||
}
|
||
|
||
private void OnRead(ushort index, byte sub)
|
||
{
|
||
switch (index)
|
||
{
|
||
case 0x4208: // Max Charging Voltage
|
||
SdoReadOk2(index, sub, RAW_MAX_VOLTAGE);
|
||
// Log($" [SDO] 0x4208 Max Voltage → {MAX_VOLTAGE_V} V (raw=0x{RAW_MAX_VOLTAGE:X4})");
|
||
break;
|
||
|
||
case 0x4212: // Max Charging Current
|
||
SdoReadOk2(index, sub, RAW_MAX_CURRENT);
|
||
// Log($" [SDO] 0x4212 Max Current → {MAX_CURRENT_A} A (raw=0x{RAW_MAX_CURRENT:X4})");
|
||
break;
|
||
|
||
default:
|
||
// Abort: object does not exist
|
||
byte[] abort = [0x80,
|
||
(byte)(index & 0xFF), (byte)(index >> 8), sub,
|
||
0x00, 0x00, 0x02, 0x06];
|
||
SendFrame(COB_SDO_TX, abort);
|
||
break;
|
||
}
|
||
CheckInitComplete();
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// XỬ LÝ TPDO9 (COB-ID 0x264) – Pin gửi mỗi 100ms
|
||
// Byte 0: ChargeControl Byte 1: SoC
|
||
// Byte 3-4: Volt Request Byte 5-6: Curr Request Byte 7: BattStatus
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void HandleTpdo9(byte[] d)
|
||
{
|
||
if (d.Length < 7) return;
|
||
byte cc = d[0];
|
||
byte soc = d[1];
|
||
ushort vReq = (ushort)(d[3] | (d[4] << 8));
|
||
ushort iReq = (ushort)(d[5] | (d[6] << 8));
|
||
byte bs = d.Length > 7 ? d[7] : (byte)0;
|
||
|
||
Console.ForegroundColor = ConsoleColor.Green;
|
||
// Console.WriteLine(
|
||
// $"[{Now}] 📦 PIN " +
|
||
// $"SoC={soc,3}% " +
|
||
// $"VReq={vReq / 256.0,6:F2}V " +
|
||
// $"IReq={iReq / 16.0,6:F2}A " +
|
||
// $"ChargeCtrl={cc} BattStat={bs}");
|
||
// Console.ResetColor();
|
||
|
||
// Pin gửi ChargeControl=0 → pin đầy hoặc có lỗi → dừng sạc
|
||
if (cc == 0 && _sdoInitDone && _chargeActive)
|
||
{
|
||
LogWarn("ChargeControl=0 → PIN ĐẦY hoặc LỖI → Tắt relay!");
|
||
_chargeActive = false;
|
||
_relayOpen = true;
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// XỬ LÝ TPDO8 (COB-ID 0x49B) – Battery Charge Control Status
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void HandleTpdo8(byte[] d)
|
||
{
|
||
if (d.Length < 2) return;
|
||
ushort s = (ushort)(d[0] | (d[1] << 8));
|
||
bool chargingNow = s == 0xC011 || s == 0xC033;
|
||
|
||
if (chargingNow && !_batteryCharging)
|
||
{
|
||
_batteryCharging = true;
|
||
LogOk($"✅ PIN ĐÃ VÀO TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
|
||
}
|
||
else if (!chargingNow && _batteryCharging)
|
||
{
|
||
_batteryCharging = false;
|
||
LogWarn($"PIN THOÁT TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
|
||
}
|
||
|
||
string desc = s switch
|
||
{
|
||
0x0033 => "SDO init OK – chờ Bit12",
|
||
0x4033 => "Standby – chờ Bit12",
|
||
0xC011 => "⚡ CHARGING ACTIVE",
|
||
0xC033 => "⚡ Charging (normal)",
|
||
0xC000 => "Pin đầy – về standby",
|
||
0xD000 => "Keep-power hết – SHUTDOWN",
|
||
_ => $"bits={s:X4}"
|
||
};
|
||
Console.ForegroundColor = ConsoleColor.Cyan;
|
||
// Console.WriteLine($"[{Now}] 📊 STATUS 0x{s:X4} → {desc}");
|
||
Console.ResetColor();
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// Kiểm tra SDO init sequence đã đủ 4 bước chưa
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void CheckInitComplete()
|
||
{
|
||
if (_sdoInitDone) return;
|
||
if (_batteryStatus == 1
|
||
&& _chargeControl == 1
|
||
&& _voltageReqRaw > 0
|
||
&& _currentReqRaw > 0)
|
||
{
|
||
_sdoInitDone = true;
|
||
// Console.ForegroundColor = ConsoleColor.Yellow;
|
||
// Console.WriteLine($"\n[{Now}] ══════════════════════════════════════");
|
||
// Console.WriteLine($"[{Now}] ✅ SDO INITIALIZATION HOÀN TẤT!");
|
||
// Console.WriteLine($"[{Now}] BatteryStatus={_batteryStatus} ChargeControl={_chargeControl}");
|
||
// Console.WriteLine($"[{Now}] VoltReq={_voltageReqRaw / 256.0:F3}V CurrReq={_currentReqRaw / 16.0:F3}A");
|
||
// Console.WriteLine($"[{Now}] ══════════════════════════════════════\n");
|
||
// Console.ResetColor();
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// SDO helpers
|
||
// ════════════════════════════════════════════════════════════════
|
||
private void SdoWriteOk(ushort index, byte sub)
|
||
{
|
||
byte[] d = [0x60, (byte)(index & 0xFF), (byte)(index >> 8), sub, 0, 0, 0, 0];
|
||
SendFrame(COB_SDO_TX, d);
|
||
// Dim($"← SDO OK 0x{COB_SDO_TX:X3} idx=0x{index:X4}");
|
||
}
|
||
|
||
private void SdoReadOk2(ushort index, byte sub, ushort value)
|
||
{
|
||
byte[] d = [0x4B,
|
||
(byte)(index & 0xFF), (byte)(index >> 8), sub,
|
||
(byte)(value & 0xFF), (byte)(value >> 8), 0, 0];
|
||
SendFrame(COB_SDO_TX, d);
|
||
// Dim($"← SDO RSP 0x{COB_SDO_TX:X3} idx=0x{index:X4} val=0x{value:X4}");
|
||
}
|
||
|
||
private void SendFrame(uint canId, byte[] data)
|
||
{
|
||
var bus = _can;
|
||
if (bus is null || !bus.IsConnected)
|
||
{
|
||
return;
|
||
}
|
||
|
||
bus.SendFrameAsync(canId, data, _ct).GetAwaiter().GetResult();
|
||
}
|
||
|
||
private bool TryReceive(out CanFrameReceivedEventArgs frame)
|
||
{
|
||
if (_rxQueue.TryDequeue(out frame!))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
try
|
||
{
|
||
if (!_rxSignal.Wait(10, _ct))
|
||
{
|
||
frame = null!;
|
||
return false;
|
||
}
|
||
}
|
||
catch (OperationCanceledException)
|
||
{
|
||
frame = null!;
|
||
return false;
|
||
}
|
||
|
||
if (_rxQueue.TryDequeue(out frame!))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
frame = null!;
|
||
return false;
|
||
}
|
||
|
||
private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
|
||
{
|
||
_rxQueue.Enqueue(e);
|
||
try
|
||
{
|
||
_rxSignal.Release();
|
||
}
|
||
catch (SemaphoreFullException)
|
||
{
|
||
}
|
||
}
|
||
|
||
// ════════════════════════════════════════════════════════════════
|
||
// Logging
|
||
// ════════════════════════════════════════════════════════════════
|
||
private static string Now => DateTime.Now.ToString("HH:mm:ss.fff");
|
||
|
||
private static void Log(string msg)
|
||
=> Console.WriteLine($"[{Now}] {msg}");
|
||
|
||
private static void LogOk(string msg)
|
||
{
|
||
Console.ForegroundColor = ConsoleColor.Yellow;
|
||
Console.WriteLine($"[{Now}] {msg}");
|
||
Console.ResetColor();
|
||
}
|
||
|
||
private static void LogWarn(string msg)
|
||
{
|
||
Console.ForegroundColor = ConsoleColor.Red;
|
||
Console.WriteLine($"[{Now}] ⚠️ {msg}");
|
||
Console.ResetColor();
|
||
}
|
||
|
||
private static void Dim(string msg)
|
||
{
|
||
Console.ForegroundColor = ConsoleColor.DarkGray;
|
||
Console.WriteLine($"[{Now}] {msg}");
|
||
Console.ResetColor();
|
||
}
|
||
}
|