112 lines
4.8 KiB
C#
112 lines
4.8 KiB
C#
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public partial class RobotPlcController
|
|
{
|
|
public OperatingMode PeripheralMode { get; private set; }
|
|
public SafetySpeed SafetySpeed { get; private set; }
|
|
|
|
public bool Emergency { get; private set; }
|
|
public bool Bumper { get; private set; }
|
|
|
|
public bool LidarFrontProtectField { get; private set; }
|
|
public bool LidarBackProtectField { get; private set; }
|
|
public bool LidarFrontTimProtectField { get; private set; }
|
|
|
|
// Lift state - now cached instead of direct read
|
|
public bool LiftedUp { get; private set; }
|
|
public bool LiftedDown { get; private set; }
|
|
public bool LiftHome { get; private set; }
|
|
|
|
// Motor state - now cached instead of direct read
|
|
public bool LeftMotorReady { get; private set; }
|
|
public bool RightMotorReady { get; private set; }
|
|
public bool LiftMotorReady { get; private set; }
|
|
|
|
public bool ButtonStart { get; private set; }
|
|
public bool ButtonStop { get; private set; }
|
|
public bool ButtonReset { get; private set; }
|
|
|
|
// Other state - now cached instead of direct read
|
|
public bool HasLoad { get; private set; }
|
|
public bool EnabledCharger { get; private set; }
|
|
public bool Charging { get; private set; }
|
|
public bool MutedBase { get; private set; }
|
|
public bool MutedLoad { get; private set; }
|
|
|
|
// Write state tracking - đọc từ write addresses của PLC
|
|
public SystemState CurrentSystemState => ReadSystemState();
|
|
public OperationState CurrentOperationState => ReadOperationState();
|
|
public RFMode CurrentRFMode => ReadRFMode();
|
|
public bool SetHorizontalLoadValue => ModbusTcpDevice?.ReadCoil(SetHorizontalLoadAddress) ?? false;
|
|
public bool SetMutedBaseValue => ModbusTcpDevice?.ReadCoil(SetMutedBaseAddress) ?? false;
|
|
public bool SetMutedLoadValue => ModbusTcpDevice?.ReadCoil(SetMutedLoadAddress) ?? false;
|
|
public bool SetEnableChargerValue => ModbusTcpDevice?.ReadCoil(EnableChargerAddress) ?? false;
|
|
public bool SetHasLoadValue => ModbusTcpDevice?.ReadCoil(SetHasLoadAddress) ?? false;
|
|
public bool SetRFEStopValue => ModbusTcpDevice?.ReadCoil(SetRFEStopAddress) ?? false;
|
|
public bool SetBatteryLowValue => ModbusTcpDevice?.ReadCoil(SetBatteryLowAddress) ?? false;
|
|
public bool SetLightOnValue => ModbusTcpDevice?.ReadCoil(SetLightOnAddress) ?? false;
|
|
|
|
private SystemState ReadSystemState()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return SystemState.INIT;
|
|
|
|
bool[] states = device.ReadCoils(RobotStateWriteAddress, 10);
|
|
if (states.Length == 10)
|
|
{
|
|
// Decode state from one-hot encoded coils
|
|
if (states[0]) return SystemState.INIT;
|
|
else if (states[1]) return SystemState.PAUSED;
|
|
else if (states[2]) return SystemState.IDLE;
|
|
else if (states[3]) return SystemState.PROCCESSING;
|
|
else if (states[4]) return SystemState.DOCKING;
|
|
else if (states[5]) return SystemState.MAINTENANCE;
|
|
else if (states[6]) return SystemState.MANUAL;
|
|
else if (states[7]) return SystemState.OVERRIDE;
|
|
else if (states[8]) return SystemState.CHARGING;
|
|
else if (states[9]) return SystemState.ERROR;
|
|
}
|
|
else Logger.Warning($"Read system state is failed: data length {states.Length} is wrong.");
|
|
return SystemState.INIT;
|
|
}
|
|
|
|
private OperationState ReadOperationState()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return OperationState.None;
|
|
|
|
bool[] states = device.ReadCoils(RobotOperationWriteAddress, 3);
|
|
if (states.Length == 3)
|
|
{
|
|
// Decode operation state from one-hot encoded coils
|
|
if (states[0]) return OperationState.Move;
|
|
else if (states[1]) return OperationState.Lifting;
|
|
else if (states[2]) return OperationState.LiftRotating;
|
|
else return OperationState.None;
|
|
}
|
|
else Logger.Warning($"Read operation state is failed: data length {states.Length} is wrong.");
|
|
return OperationState.None;
|
|
}
|
|
|
|
private RFMode ReadRFMode()
|
|
{
|
|
var device = ModbusTcpDevice;
|
|
if (device is null) return RFMode.None;
|
|
|
|
bool[] modes = device.ReadCoils(SetRFModeAddress, 3);
|
|
if (modes.Length == 3)
|
|
{
|
|
// Decode RF mode from one-hot encoded coils
|
|
if (modes[0]) return RFMode.Default;
|
|
else if (modes[1]) return RFMode.Maintenance;
|
|
else if (modes[2]) return RFMode.Override;
|
|
else return RFMode.None;
|
|
}
|
|
else Logger.Warning($"Read RF mode is failed: data length {modes.Length} is wrong.");
|
|
return RFMode.None;
|
|
}
|
|
}
|