467 lines
20 KiB
C#
467 lines
20 KiB
C#
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public partial class RobotPlcController(IRobotConfiguration RobotConfiguration, IDeviceProvider DeviceProvider, Logger<RobotPlcController> Logger) : IPlcController
|
|
{
|
|
public bool IsReady { get; private set; } = false;
|
|
public bool IsDisconected => !IsSimulation && (ModbusTcpDevice is null || !ModbusTcpDevice.IsConnected);
|
|
public event Action<SafetySpeed>? OnSafetySpeedChanged;
|
|
public event Action<OperatingMode>? OnPeripheralModeChanged;
|
|
public event Action<PeripheralButton>? OnButtonPressed;
|
|
public event Action<StopStateType>? OnStop;
|
|
|
|
private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;
|
|
|
|
private IModbusTcpDevice? ModbusTcpDevice;
|
|
|
|
// Edge detection tracking fields
|
|
private StopStateType _lastStopState = StopStateType.None;
|
|
private bool _lastButtonStart, _lastButtonReset, _lastButtonStop;
|
|
|
|
public async Task Start(CancellationToken cancellationToken)
|
|
{
|
|
LidarBackProtectField = true;
|
|
LidarFrontProtectField = true;
|
|
if (IsSimulation)
|
|
{
|
|
PeripheralMode = OperatingMode.AUTOMATIC;
|
|
}
|
|
else if (ModbusTcpDevice is null)
|
|
{
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (DeviceProvider.AreDevicesLoaded) break;
|
|
await Task.Delay(500);
|
|
}
|
|
|
|
var device = DeviceProvider.GetDevice("plc-001");
|
|
if (device is IModbusTcpDevice modbusDevice)
|
|
{
|
|
ModbusTcpDevice = modbusDevice;
|
|
ModbusTcpDevice.DataRegisterChanged += ModbusDataChanged;
|
|
while (!cancellationToken.IsCancellationRequested)
|
|
{
|
|
if (modbusDevice.IsConnected) break;
|
|
await Task.Delay(500);
|
|
}
|
|
}
|
|
else return;
|
|
}
|
|
IsReady = true;
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
|
|
ModbusTcpDevice?.DataRegisterChanged -= ModbusDataChanged;
|
|
ModbusTcpDevice = null;
|
|
|
|
// Reset edge detection state
|
|
_lastStopState = StopStateType.None;
|
|
_lastButtonStart = false;
|
|
_lastButtonReset = false;
|
|
_lastButtonStop = false;
|
|
|
|
IsReady = false;
|
|
}
|
|
|
|
private void ModbusDataChanged(ModbusRegisterType type)
|
|
{
|
|
if (type == ModbusRegisterType.Coil)
|
|
{
|
|
// Read-only data from PLC
|
|
ReadSafetyProtect();
|
|
ReadSafetySpeed();
|
|
ReadButton();
|
|
ReadSwitch();
|
|
ReadLiftState();
|
|
ReadMotorState();
|
|
ReadOtherState();
|
|
}
|
|
}
|
|
|
|
public void SetHorizontalLoad(bool value)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetHorizontalLoadAddress, value, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetMutedBase(bool muted)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetMutedBaseAddress, muted, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetMutedLoad(bool muted)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetMutedLoadAddress, muted, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
/// <summary>Bật/tắt đèn — ghi coil M918 (TCP address 2966).</summary>
|
|
public void SetLightOn(bool value)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetLightOnAddress, value, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetOperationState(OperationState state)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = Task.Run(async () =>
|
|
{
|
|
switch (state)
|
|
{
|
|
case OperationState.Move:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteMoveState);
|
|
break;
|
|
case OperationState.Lifting:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftingState);
|
|
break;
|
|
case OperationState.LiftRotating:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteLiftRotatingState);
|
|
break;
|
|
case OperationState.None:
|
|
default:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotOperationWriteAddress, RobotExecuteClearState);
|
|
break;
|
|
}
|
|
});
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetSystemState(SystemState state)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = Task.Run(async () =>
|
|
{
|
|
switch (state)
|
|
{
|
|
case SystemState.INIT:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotInitState);
|
|
break;
|
|
case SystemState.PAUSED:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotPauseState);
|
|
break;
|
|
case SystemState.IDLE:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotIdleState);
|
|
break;
|
|
case SystemState.PROCCESSING:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotProccessingState);
|
|
break;
|
|
case SystemState.DOCKING:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotDockingState);
|
|
break;
|
|
case SystemState.MAINTENANCE:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotMaintenanceState);
|
|
break;
|
|
case SystemState.MANUAL:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotManualState);
|
|
break;
|
|
case SystemState.OVERRIDE:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotOverrideState);
|
|
break;
|
|
case SystemState.CHARGING:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotCharingState);
|
|
break;
|
|
case SystemState.ERROR:
|
|
await ModbusTcpDevice.WriteCoilsAsync(RobotStateWriteAddress, RobotErrorState);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
});
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetEnableCharger(bool value)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(EnableChargerAddress, value, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetRFMode(RFMode mode)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = Task.Run(async () =>
|
|
{
|
|
switch (mode)
|
|
{
|
|
case RFMode.Default:
|
|
await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeDefault);
|
|
break;
|
|
case RFMode.Maintenance:
|
|
await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeMaintenance);
|
|
break;
|
|
case RFMode.Override:
|
|
await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeOverride);
|
|
break;
|
|
case RFMode.None:
|
|
default:
|
|
await ModbusTcpDevice.WriteCoilsAsync(SetRFModeAddress, RFModeNone);
|
|
break;
|
|
}
|
|
});
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetHasLoad(bool hasLoad)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetHasLoadAddress, hasLoad, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetRFEStop(bool stop)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetRFEStopAddress, stop, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
public void SetBatteryLow(bool value)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected. Ensure Start() completed successfully.");
|
|
var write = ModbusTcpDevice.WriteCoilAsync(SetBatteryLowAddress, value, CancellationToken.None);
|
|
write.Wait();
|
|
}
|
|
|
|
/// <summary>Ghi hướng di chuyển xuống PLC: tiến M931, lùi M932; không đi thì cả hai off.</summary>
|
|
public void SetDirectionForwardBackward(bool forward, bool backward)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) return;
|
|
// Tiến: M931 on, M932 off. Lùi: M931 off, M932 on. Không đi: cả hai off (không bao giờ cả hai on)
|
|
var w1 = ModbusTcpDevice.WriteCoilAsync(DirectionForwardAddress, forward, CancellationToken.None);
|
|
var w2 = ModbusTcpDevice.WriteCoilAsync(DirectionBackwardAddress, backward, CancellationToken.None);
|
|
Task.WaitAll(w1, w2);
|
|
}
|
|
|
|
/// <summary>Ghi M815 Alarm Reset xuống PLC (pulse) — gửi ngay ON rồi OFF, cùng coil như khi bấm M815 trên device.</summary>
|
|
public void WriteAlarmResetM815()
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) return;
|
|
ushort addr = AlarmResetM815WriteAddress;
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
|
|
Thread.Sleep(150);
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
|
|
Logger.Info($"WriteAlarmResetM815: pulsed coil {addr} (M815) ON -> OFF");
|
|
}
|
|
|
|
/// <summary>Lift: Homing — pulse coil M933.</summary>
|
|
public void LiftHoming()
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
|
|
ushort addr = LiftHomingAddress;
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(addr, true, CancellationToken.None).GetAwaiter().GetResult();
|
|
Thread.Sleep(150);
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(addr, false, CancellationToken.None).GetAwaiter().GetResult();
|
|
Logger.Info($"LiftHoming: pulsed coil {addr} (M933) ON -> OFF");
|
|
}
|
|
|
|
/// <summary>Lift: Điều khiển velocity — lên (M934), xuống (M935); cả hai off = dừng.</summary>
|
|
public void SetLiftVelocity(bool up, bool down)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) return;
|
|
var w1 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityUpAddress, up, CancellationToken.None);
|
|
var w2 = ModbusTcpDevice.WriteCoilAsync(LiftVelocityDownAddress, down, CancellationToken.None);
|
|
Task.WaitAll(w1, w2);
|
|
}
|
|
|
|
/// <summary>Lift: Ghi vị trí đích (10000 = 0.01m) vào 2 holding registers rồi pulse M936.</summary>
|
|
public void SetLiftPositionAndGo(int position)
|
|
{
|
|
if (IsSimulation) return;
|
|
if (ModbusTcpDevice is null) throw new InvalidOperationException("PLC Controller is not connected.");
|
|
var high = (ushort)((position >> 16) & 0xFFFF);
|
|
var low = (ushort)(position & 0xFFFF);
|
|
ModbusTcpDevice.WriteHoldingRegistersAsync(LiftTargetPositionRegister, [high, low], CancellationToken.None).GetAwaiter().GetResult();
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, true, CancellationToken.None).GetAwaiter().GetResult();
|
|
Thread.Sleep(150);
|
|
ModbusTcpDevice.WriteCoilImmediateAsync(LiftGoToPositionAddress, false, CancellationToken.None).GetAwaiter().GetResult();
|
|
Logger.Info($"SetLiftPositionAndGo: position={position} (10000=0.01m), pulsed M936");
|
|
}
|
|
|
|
private static SafetySpeed ReadSpeed(bool[] flag)
|
|
{
|
|
if (flag.Length < 7) return SafetySpeed.Very_Slow;
|
|
if (flag[0]) return SafetySpeed.Very_Slow; // giới hạn chặt nhất
|
|
if (flag[1]) return SafetySpeed.Slow;
|
|
if (flag[2]) return SafetySpeed.Normal;
|
|
if (flag[3]) return SafetySpeed.Medium;
|
|
if (flag[4]) return SafetySpeed.Optimal;
|
|
if (flag[5]) return SafetySpeed.Fast;
|
|
if (flag[6]) return SafetySpeed.Very_Fast;
|
|
|
|
return SafetySpeed.Very_Fast; // không có giới hạn nào
|
|
}
|
|
|
|
private void ReadSafetySpeed()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
bool[] speed = device.ReadCoils(SpeedLimitReadAddress, SpeedRange);
|
|
if (speed.Length == SpeedRange)
|
|
{
|
|
var activeSpeed = ReadSpeed(speed);
|
|
if (activeSpeed != SafetySpeed)
|
|
{
|
|
SafetySpeed = activeSpeed;
|
|
OnSafetySpeedChanged?.Invoke(activeSpeed);
|
|
}
|
|
}
|
|
else Logger.Warning($"Read Safety Speed is failed: data length {speed.Length} is wrong.");
|
|
}
|
|
|
|
private void ReadButton()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
bool[] buttons = device.ReadCoils((ushort)(ReadOnlyStartAddress + StartButtonOffsetAddress), 3);
|
|
if (buttons.Length == 3)
|
|
{
|
|
var newStart = buttons[0];
|
|
var newReset = buttons[1];
|
|
var newStop = buttons[2];
|
|
|
|
// Rising edge detection - only fire when button state changes from false to true
|
|
if (newStart && !_lastButtonStart) OnButtonPressed?.Invoke(PeripheralButton.Start);
|
|
if (newReset && !_lastButtonReset) OnButtonPressed?.Invoke(PeripheralButton.Reset);
|
|
if (newStop && !_lastButtonStop) OnButtonPressed?.Invoke(PeripheralButton.Stop);
|
|
|
|
// Update tracking state
|
|
_lastButtonStart = newStart;
|
|
_lastButtonReset = newReset;
|
|
_lastButtonStop = newStop;
|
|
|
|
// Update public properties
|
|
ButtonStart = newStart;
|
|
ButtonReset = newReset;
|
|
ButtonStop = newStop;
|
|
}
|
|
else Logger.Warning($"Read button is failed: data length {buttons.Length} is wrong.");
|
|
}
|
|
|
|
private void ReadSwitch()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
bool[] switchs = device.ReadCoils((ushort)(ReadOnlyStartAddress + SwitchLockOffsetAddress), 3);
|
|
if (switchs.Length == 3)
|
|
{
|
|
var oldMode = PeripheralMode;
|
|
if (switchs[0])
|
|
{
|
|
PeripheralMode = OperatingMode.SERVICE;
|
|
}
|
|
else if (switchs[1])
|
|
{
|
|
PeripheralMode = OperatingMode.AUTOMATIC;
|
|
}
|
|
else if (switchs[2])
|
|
{
|
|
PeripheralMode = OperatingMode.MANUAL;
|
|
}
|
|
if (oldMode != PeripheralMode) OnPeripheralModeChanged?.Invoke(PeripheralMode);
|
|
}
|
|
else Logger.Warning($"Read switch mode is failed: data length {switchs.Length} is wrong.");
|
|
}
|
|
|
|
private void ReadSafetyProtect()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
bool[] sensors = device.ReadCoils((ushort)(ReadOnlyStartAddress + EmergencyOffsetAddress), 5);
|
|
if (sensors.Length == 5)
|
|
{
|
|
Emergency = sensors[0];
|
|
Bumper = sensors[1];
|
|
LidarFrontProtectField = sensors[2];
|
|
LidarBackProtectField = sensors[3];
|
|
LidarFrontTimProtectField = sensors[4];
|
|
|
|
// Determine current stop state
|
|
StopStateType currentState;
|
|
if (Emergency) currentState = StopStateType.EMC;
|
|
else if (Bumper) currentState = StopStateType.Bumper;
|
|
else currentState = StopStateType.None;
|
|
|
|
// Only fire event when state actually changes
|
|
if (currentState != _lastStopState)
|
|
{
|
|
_lastStopState = currentState;
|
|
OnStop?.Invoke(currentState);
|
|
}
|
|
}
|
|
else Logger.Warning($"Read safety protect is failed: data length {sensors.Length} is wrong.");
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update lift state from Modbus cache
|
|
/// </summary>
|
|
private void ReadLiftState()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
LiftedUp = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedUpOffsetAddress));
|
|
LiftedDown = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftedDownOffsetAddress));
|
|
LiftHome = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftHomeOffsetAddress));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update motor ready state from Modbus cache
|
|
/// </summary>
|
|
private void ReadMotorState()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
LeftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LeftMotorReadyOffsetAddress));
|
|
RightMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + RightMotorReadyOffsetAddress));
|
|
LiftMotorReady = device.ReadCoil((ushort)(ReadOnlyStartAddress + LiftMotorReadyOffsetAddress));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Update other state from Modbus cache
|
|
/// </summary>
|
|
private void ReadOtherState()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return;
|
|
|
|
HasLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + HasLoadOffsetAddress));
|
|
EnabledCharger = device.ReadCoil((ushort)(ReadOnlyStartAddress + EnabledChargerOffsetAddress));
|
|
Charging = device.ReadCoil((ushort)(ReadOnlyStartAddress + ResponseChargingOffsetAddress));
|
|
MutedBase = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedBaseOffsetAddress));
|
|
MutedLoad = device.ReadCoil((ushort)(ReadOnlyStartAddress + MutedLoadOffsetAddress));
|
|
}
|
|
}
|