465 lines
19 KiB
C#
465 lines
19 KiB
C#
using MegaRobo.Connections;
|
||
using MegaRobo.Connections.Adapters;
|
||
using MegaRobo.Connections.Modbus;
|
||
using MegaRobo.Connections.Sockets.Tcp;
|
||
using MegaRobo.Contract;
|
||
using MegaRobo.ControlDevices.Abstractions;
|
||
using MegaRobo.Logger;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using System.Net;
|
||
using System.Text;
|
||
using System.Threading.Tasks;
|
||
using static MegaRobo.Connections.Modbus.ModbusParams;
|
||
|
||
namespace MegaRobo.C00225155.ControlDevices
|
||
{
|
||
/// <summary>
|
||
/// 高温震荡电机驱动器控制服务
|
||
/// </summary>
|
||
public class HighTemperatureMotorControlService : ConnectorBase<MegaTcpClient>
|
||
{
|
||
/// <summary>
|
||
/// 寄存器地址枚举（十六进制转十进制，Modbus 需传入十进制地址）
|
||
/// </summary>
|
||
private enum MotorRegisterAddress : ushort
|
||
{
|
||
启停控制 = 0x00, // 0x00（十进制0）：启停控制
|
||
工作模式 = 0x02, // 0x02（十进制2）：模式切换
|
||
速度指令 = 0x06, // 0x06（十进制6）：速度指令
|
||
清除故障 = 0x4A, // 0x4A（十进制74）：清除故障
|
||
驱动器状态 = 0xE3, // 0xE3（十进制227）：驱动器状态
|
||
电机转速 = 0xE4 // 0xE4（十进制228）：电机实际转速
|
||
}
|
||
|
||
/// <summary>
|
||
/// 写入数据枚举
|
||
/// </summary>
|
||
private enum MotorWriteData : ushort
|
||
{
|
||
停止 = 0x0000, // 启停控制：0=停止
|
||
启动 = 0x0001, // 启停控制：1=启动
|
||
速度模式 = 0x00C4, // 工作模式：0x00C4=速度模式
|
||
位置模式 = 0x00D0 // 工作模式：0x00D0=位置模式
|
||
}
|
||
|
||
/// <summary>
|
||
/// 驱动器状态位解析（对应 0xE3 寄存器的 bit 位）
|
||
/// </summary>
|
||
public class MotorState
|
||
{
|
||
public bool 运行状态 { get; set; } // bit0：0=停机，1=启动
|
||
public bool 过流故障 { get; set; } // bit1：1=故障
|
||
public bool 过压故障 { get; set; } // bit2：1=故障
|
||
public bool 编码器故障 { get; set; } // bit3：1=故障
|
||
public bool 位置偏差过大 { get; set; }// bit4：1=故障
|
||
public bool 欠压故障 { get; set; } // bit5：1=故障
|
||
public bool 过载故障 { get; set; } // bit6：1=故障
|
||
public bool 外部控制标志 { get; set; }// bit7：0=PC控制，1=PLC控制
|
||
public string 故障描述 { get; set; } // 汇总故障信息
|
||
}
|
||
|
||
public override async Task<string> Initialize(ICoreService coreService, params object[] args)
|
||
{
|
||
CoreService = coreService;
|
||
try
|
||
{
|
||
base.CoreService = coreService;
|
||
if (args is [IPEndPoint remoteHost, ..])
|
||
{
|
||
base.ConnectorAdapter = new MegaTcpClient
|
||
{
|
||
Logger = coreService.Logger,
|
||
Name = base.Name
|
||
};
|
||
if (base.ConnectorAdapter != null)
|
||
{
|
||
base.ConnectorAdapter.DataReceived += ConnectorOnDataReceived;
|
||
}
|
||
|
||
ConnectionState = await base.ConnectorAdapter.ConnectAsync(remoteHost);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex);
|
||
return await Task.FromResult(ex.Message);
|
||
}
|
||
return string.Empty;
|
||
}
|
||
|
||
private void ConnectorOnDataReceived(object sender, TcpMessage tcpMsg)
|
||
{
|
||
if (tcpMsg is null || tcpMsg.Data == null || tcpMsg.Data.Length == 0)
|
||
return;
|
||
string hexData = BitConverter.ToString(tcpMsg.Data).Replace("-", " ");
|
||
Logger.LogInformation($"接收到电机驱动器数据（十六进制）：{hexData}");
|
||
}
|
||
|
||
public override Task<ConnectionState> ReconnectAsync()
|
||
{
|
||
return base.ReconnectAsync();
|
||
}
|
||
|
||
public override async Task<ConnectionState> CheckConnected()
|
||
{
|
||
var state = ConnectionState.None;
|
||
if (ConnectorAdapter is not null)
|
||
{
|
||
state = await ConnectorAdapter.CheckConnectedAsync();
|
||
}
|
||
ConnectionState = state;
|
||
return state;
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 电机启停控制
|
||
/// </summary>
|
||
/// <param name="id">设备从站号</param>
|
||
/// <param name="isStart"></param>
|
||
/// <returns></returns>
|
||
public async Task<bool> ControlMotorStartStop(byte id, bool isStart)
|
||
{
|
||
try
|
||
{
|
||
// 1. 确定写入数据：启动=0x0001，停止=0x0000
|
||
MotorWriteData writeData = isStart ? MotorWriteData.启动 : MotorWriteData.停止;
|
||
// 2. 调用 Modbus 写单个寄存器（功能码06），地址=启停控制（0x00），字节顺序AB（高字节在前）
|
||
var writeResult = ModbusRTU.WriteRegisterSingle(
|
||
deviceAdress: id, // 从站地址（默认1）
|
||
startAdress: (ushort)MotorRegisterAddress.启停控制,
|
||
lstData: (ushort)writeData,
|
||
byteOrder: ByteOrder.AB // 协议要求高字节在前
|
||
);
|
||
|
||
if (!writeResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"启停控制指令构建失败：{writeResult.Message}");
|
||
return false;
|
||
}
|
||
|
||
// 3. 发送指令并获取响应
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(writeResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("启停控制未收到响应");
|
||
return false;
|
||
}
|
||
|
||
// 4. 验证响应（协议要求写成功后返回原报文）
|
||
bool isResponseMatch = CompareBytes(writeResult.Respond, tcpMessage.Data);
|
||
if (!isResponseMatch)
|
||
{
|
||
Logger.LogError($"启停控制响应不匹配：发送={BitConverter.ToString(writeResult.Respond)}, 接收={BitConverter.ToString(tcpMessage.Data)}");
|
||
return false;
|
||
}
|
||
|
||
Logger.LogInformation($"{(isStart ? "启动" : "停止")}电机成功");
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex, null, LogEvent.Error, null, LogLevel.Error, "HighTemperatureMotorControlService", "ControlMotorStartStop", 150, false);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 设置电机速度（速度模式下）
|
||
/// </summary>
|
||
/// <param name="id">设备从站号</param>
|
||
/// <param name="targetSpeed">目标速度（0-3000 RPM，协议上限3000）</param>
|
||
public async Task<bool> SetMotorSpeed(byte id, int targetSpeed)
|
||
{
|
||
try
|
||
{
|
||
// 1. 参数校验：速度范围 0-3000 RPM（协议规定）
|
||
if (targetSpeed < 0 || targetSpeed > 3000)
|
||
{
|
||
Logger.LogError($"速度值超出范围（0-3000 RPM）：{targetSpeed}");
|
||
return false;
|
||
}
|
||
|
||
// 2. 切换到速度模式（必须先设置模式，再写速度）
|
||
bool modeSuccess = await SwitchToSpeedMode(id);
|
||
if (!modeSuccess)
|
||
{
|
||
Logger.LogError("切换速度模式失败，无法设置速度");
|
||
return false;
|
||
}
|
||
|
||
// 3. 计算速度指令值：写入值 = (目标速度 / 3000) * 8192（协议公式）
|
||
ushort speedCommand = (ushort)Math.Round((targetSpeed / 3000.0) * 8192);
|
||
Logger.LogInformation($"目标速度={targetSpeed} RPM，计算后指令值=0x{speedCommand:X4}");
|
||
|
||
// 4. 写速度指令到寄存器 0x06（功能码06）
|
||
var writeResult = ModbusRTU.WriteRegisterSingle(
|
||
deviceAdress: id,
|
||
startAdress: (ushort)MotorRegisterAddress.速度指令,
|
||
lstData: speedCommand,
|
||
byteOrder: ByteOrder.AB // 高字节在前
|
||
);
|
||
|
||
if (!writeResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"速度指令构建失败：{writeResult.Message}");
|
||
return false;
|
||
}
|
||
|
||
// 5. 发送指令并验证响应
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(writeResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("设置速度未收到响应");
|
||
return false;
|
||
}
|
||
|
||
// 6. 验证响应（写成功返回原报文）
|
||
bool isResponseMatch = CompareBytes(writeResult.Respond, tcpMessage.Data);
|
||
if (!isResponseMatch)
|
||
{
|
||
Logger.LogError($"速度设置响应不匹配：发送={BitConverter.ToString(writeResult.Respond)}, 接收={BitConverter.ToString(tcpMessage.Data)}");
|
||
return false;
|
||
}
|
||
|
||
Logger.LogInformation($"设置电机速度成功：{targetSpeed} RPM（指令值=0x{speedCommand:X4}）");
|
||
return true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex, null, LogEvent.Error, null, LogLevel.Error, "HighTemperatureMotorControlService", "SetMotorSpeed", 150, false);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取电机驱动器状态（运行状态+故障信息）
|
||
/// </summary>
|
||
/// <param name="id">设备从站号</param>
|
||
public async Task<MotorState> GetMotorState(byte id)
|
||
{
|
||
try
|
||
{
|
||
// 1. 构建读状态指令：地址0xE3（十进制227），读1个寄存器（状态是16位）
|
||
var readResult = ModbusRTU.GetReadMessage(
|
||
slaveStation: id,
|
||
readType: ModbusReadCodes.READ_HOLDING_REGISTERS, // 功能码03
|
||
startAdr: (ushort)MotorRegisterAddress.驱动器状态,
|
||
length: 1 // 状态寄存器是1个16位寄存器，长度=1
|
||
);
|
||
|
||
if (!readResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"读状态指令构建失败：{readResult.Message}");
|
||
return new MotorState { 故障描述 = "指令构建失败" };
|
||
}
|
||
|
||
// 2. 发送指令并获取响应
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(readResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("读状态未收到响应");
|
||
return new MotorState { 故障描述 = "未收到响应" };
|
||
}
|
||
|
||
// 3. 解析响应（16位状态字，字节顺序AB）
|
||
var parseResult = ModbusRTU.ReadRegisterUShort(tcpMessage.Data, ByteOrder.AB);
|
||
if (!parseResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"状态解析失败：{parseResult.Message}");
|
||
return new MotorState { 故障描述 = parseResult.Message };
|
||
}
|
||
|
||
// 4. 解析状态字的各个bit位（协议表5-22）
|
||
ushort statusWord = parseResult.Respond[0];
|
||
var motorState = new MotorState
|
||
{
|
||
运行状态 = (statusWord & (1 << 0)) != 0, // bit0
|
||
过流故障 = (statusWord & (1 << 1)) != 0, // bit1
|
||
过压故障 = (statusWord & (1 << 2)) != 0, // bit2
|
||
编码器故障 = (statusWord & (1 << 3)) != 0, // bit3
|
||
位置偏差过大 = (statusWord & (1 << 4)) != 0,// bit4
|
||
欠压故障 = (statusWord & (1 << 5)) != 0, // bit5
|
||
过载故障 = (statusWord & (1 << 6)) != 0, // bit6
|
||
外部控制标志 = (statusWord & (1 << 7)) != 0 // bit7
|
||
};
|
||
|
||
// 5. 汇总故障描述
|
||
var errors = new List<string>();
|
||
if (motorState.过流故障) errors.Add("过流");
|
||
if (motorState.过压故障) errors.Add("过压");
|
||
if (motorState.编码器故障) errors.Add("编码器故障");
|
||
if (motorState.位置偏差过大) errors.Add("位置偏差过大");
|
||
if (motorState.欠压故障) errors.Add("欠压");
|
||
if (motorState.过载故障) errors.Add("过载");
|
||
|
||
motorState.故障描述 = errors.Count > 0
|
||
? $"故障：{string.Join("、", errors)}"
|
||
: "无故障";
|
||
|
||
Logger.LogInformation($"电机状态：运行状态={(motorState.运行状态 ? "启动" : "停止")}，{motorState.故障描述}");
|
||
return motorState;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex, null, LogEvent.Error, null, LogLevel.Error, "HighTemperatureMotorControlService", "GetMotorState", 150, false);
|
||
return new MotorState { 故障描述 = $"异常：{ex.Message}" };
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 获取电机实际转速
|
||
/// </summary>
|
||
/// <param name="id">设备从站号</param>
|
||
public async Task<int> GetMotorActualSpeed(byte id)
|
||
{
|
||
try
|
||
{
|
||
// 1. 构建读转速指令：地址0xE4（十进制228），读1个寄存器
|
||
var readResult = ModbusRTU.GetReadMessage(
|
||
slaveStation: id,
|
||
readType: ModbusReadCodes.READ_HOLDING_REGISTERS,
|
||
startAdr: (ushort)MotorRegisterAddress.电机转速,
|
||
length: 1
|
||
);
|
||
|
||
if (!readResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"读转速指令构建失败：{readResult.Message}");
|
||
return -1;
|
||
}
|
||
|
||
// 2. 发送指令并获取响应
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(readResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("读转速未收到响应");
|
||
return -1;
|
||
}
|
||
|
||
// 3. 解析转速（16位无符号数，直接表示RPM）
|
||
var parseResult = ModbusRTU.ReadRegisterUShort(tcpMessage.Data, ByteOrder.AB);
|
||
if (!parseResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"转速解析失败：{parseResult.Message}");
|
||
return -1;
|
||
}
|
||
|
||
int actualSpeed = parseResult.Respond[0];
|
||
Logger.LogInformation($"电机实际转速：{actualSpeed} RPM");
|
||
return actualSpeed;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex, null, LogEvent.Error, null, LogLevel.Error, "HighTemperatureMotorControlService", "GetMotorActualSpeed", 150, false);
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 清除电机故障
|
||
/// </summary>
|
||
/// <param name="id">设备从站号</param>
|
||
/// <returns></returns>
|
||
public async Task<bool> ClearMotorFault(byte id)
|
||
{
|
||
try
|
||
{
|
||
// 协议规定：写入任意值到0x4A即可清除故障，此处写0x0000
|
||
var writeResult = ModbusRTU.WriteRegisterSingle(
|
||
deviceAdress: id,
|
||
startAdress: (ushort)MotorRegisterAddress.清除故障,
|
||
lstData: (ushort)0x0000,
|
||
byteOrder: ByteOrder.AB
|
||
);
|
||
|
||
if (!writeResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"清除故障指令构建失败：{writeResult.Message}");
|
||
return false;
|
||
}
|
||
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(writeResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("清除故障未收到响应");
|
||
return false;
|
||
}
|
||
|
||
bool isResponseMatch = CompareBytes(writeResult.Respond, tcpMessage.Data);
|
||
if (isResponseMatch)
|
||
{
|
||
Logger.LogInformation("清除电机故障成功");
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
Logger.LogError("清除故障响应不匹配");
|
||
return false;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Logger.LogException(ex, null, LogEvent.Error, null, LogLevel.Error, "HighTemperatureMotorControlService", "ClearMotorFault", 150, false);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
|
||
/// <summary>
|
||
/// 切换到速度模式（内部调用，设置模式寄存器0x02为0x00C4）
|
||
/// </summary>
|
||
/// <param name="id"></param>
|
||
/// <returns></returns>
|
||
private async Task<bool> SwitchToSpeedMode(byte id)
|
||
{
|
||
var writeResult = ModbusRTU.WriteRegisterSingle(
|
||
deviceAdress: id,
|
||
startAdress: (ushort)MotorRegisterAddress.工作模式,
|
||
lstData: (ushort)MotorWriteData.速度模式,
|
||
byteOrder: ByteOrder.AB
|
||
);
|
||
|
||
if (!writeResult.IsSuccessed)
|
||
{
|
||
Logger.LogError($"切换速度模式指令构建失败：{writeResult.Message}");
|
||
return false;
|
||
}
|
||
|
||
var tcpMessage = await ConnectorAdapter.WriteAndGetReplyAsync(writeResult.Respond, TimeSpan.FromSeconds(5));
|
||
if (tcpMessage == null || tcpMessage.Data == null)
|
||
{
|
||
Logger.LogError("切换速度模式未收到响应");
|
||
return false;
|
||
}
|
||
|
||
bool isMatch = CompareBytes(writeResult.Respond, tcpMessage.Data);
|
||
if (isMatch)
|
||
{
|
||
Logger.LogInformation("切换到速度模式成功");
|
||
return true;
|
||
}
|
||
else
|
||
{
|
||
Logger.LogError("切换速度模式响应不匹配");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 比较两个字节数组是否相等（验证响应报文）
|
||
/// </summary>
|
||
private bool CompareBytes(byte[] source, byte[] target)
|
||
{
|
||
if (source.Length != target.Length) return false;
|
||
for (int i = 0; i < source.Length; i++)
|
||
{
|
||
if (source[i] != target[i]) return false;
|
||
}
|
||
return true;
|
||
}
|
||
}
|
||
}
|