Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Modules/RobotPlcController.cs

@@ -0,0 +1,466 @@
+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));
+    }
+}