206 lines
7.7 KiB
C++
206 lines
7.7 KiB
C++
#include "RobotProtocol.h"
|
||
#include "VrSimpleLog.h"
|
||
#include "VrError.h"
|
||
#include <cstring>
|
||
|
||
RobotProtocol::RobotProtocol()
|
||
: m_pModbusServer(nullptr)
|
||
, m_bServerRunning(false)
|
||
, m_nPort(502)
|
||
, m_robotStatus(STATUS_DISCONNECTED)
|
||
{
|
||
// 初始化坐标
|
||
memset(&m_robotCoordinate, 0, sizeof(m_robotCoordinate));
|
||
}
|
||
|
||
RobotProtocol::~RobotProtocol()
|
||
{
|
||
Deinitialize();
|
||
}
|
||
|
||
int RobotProtocol::Initialize(uint16_t port)
|
||
{
|
||
if (m_bServerRunning) {
|
||
LOG_WARNING("RobotProtocol::Initialize - 服务已经在运行中\n");
|
||
return SUCCESS;
|
||
}
|
||
|
||
m_nPort = port;
|
||
|
||
// 创建ModbusTCP服务器实例
|
||
bool bRet = IYModbusTCPServer::CreateInstance(&m_pModbusServer);
|
||
LOG_ERRO("RobotProtocol::%s - 创建ModbusTCP服务器%s \n", __func__, bRet ? "失败" : "成功");
|
||
m_bServerRunning = bRet;
|
||
ERR_CODE_RETURN(NET_ERR_CREAT_INIT);
|
||
|
||
// 设置初始状态
|
||
m_robotStatus = STATUS_CONNECTED;
|
||
|
||
LOG_INFO("RobotProtocol::Initialize - ModbusTCP服务初始化完成\n");
|
||
return 0;
|
||
}
|
||
|
||
void RobotProtocol::Deinitialize()
|
||
{
|
||
LOG_DEBUG("RobotProtocol::Deinitialize - 停止ModbusTCP服务\n");
|
||
|
||
// 停止ModbusTCP服务器
|
||
StopModbusTCPServer();
|
||
|
||
// 重置状态
|
||
m_robotStatus = STATUS_DISCONNECTED;
|
||
m_bServerRunning = false;
|
||
|
||
LOG_INFO("RobotProtocol::Deinitialize - ModbusTCP服务已停止\n");
|
||
}
|
||
|
||
int RobotProtocol::SetRobotCoordinate(const RobotCoordinate& coordinate)
|
||
{
|
||
LOG_DEBUG("RobotProtocol::SetRobotCoordinate - 设置机械臂坐标\n");
|
||
LOG_DEBUG("坐标: X=%.2f, Y=%.2f, Z=%.2f\n", coordinate.x, coordinate.y, coordinate.z);
|
||
LOG_DEBUG("旋转: RX=%.2f, RY=%.2f, RZ=%.2f\n", coordinate.rx, coordinate.ry, coordinate.rz);
|
||
|
||
m_robotCoordinate = coordinate;
|
||
|
||
return 0;
|
||
}
|
||
|
||
int RobotProtocol::GetRobotCoordinate(RobotCoordinate& coordinate)
|
||
{
|
||
coordinate = m_robotCoordinate;
|
||
return 0;
|
||
}
|
||
|
||
RobotProtocol::RobotStatus RobotProtocol::GetDetectionStatus() const
|
||
{
|
||
return m_robotStatus;
|
||
}
|
||
|
||
void RobotProtocol::SetConnectionCallback(const ConnectionCallback& callback)
|
||
{
|
||
m_connectionCallback = callback;
|
||
}
|
||
|
||
void RobotProtocol::SetWorkSignalCallback(const WorkSignalCallback& callback)
|
||
{
|
||
m_workSignalCallback = callback;
|
||
}
|
||
|
||
bool RobotProtocol::IsRunning() const
|
||
{
|
||
return m_bServerRunning;
|
||
}
|
||
|
||
void RobotProtocol::StopModbusTCPServer()
|
||
{
|
||
LOG_DEBUG("RobotProtocol::StopModbusTCPServer - 停止ModbusTCP服务器\n");
|
||
|
||
if (m_pModbusServer) {
|
||
// 释放资源
|
||
delete m_pModbusServer;
|
||
m_pModbusServer = nullptr;
|
||
}
|
||
|
||
m_bServerRunning = false;
|
||
LOG_INFO("RobotProtocol::StopModbusTCPServer - ModbusTCP服务器已停止\n");
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteCoils(uint8_t unitId, uint16_t startAddress,
|
||
uint16_t quantity, const uint8_t* values)
|
||
{
|
||
LOG_DEBUG("RobotProtocol::OnWriteCoils - 单元ID:%d, 起始地址:%d, 数量:%d\n",
|
||
unitId, startAddress, quantity);
|
||
|
||
// 处理工作信号线圈
|
||
if (startAddress <= WORK_SIGNAL_COIL && startAddress + quantity > WORK_SIGNAL_COIL) {
|
||
uint16_t coilIndex = WORK_SIGNAL_COIL - startAddress;
|
||
bool workSignal = (values[coilIndex / 8] >> (coilIndex % 8)) & 0x01;
|
||
|
||
LOG_INFO("RobotProtocol::OnWriteCoils - 收到工作信号:%s\n",
|
||
(workSignal ? "开始工作" : "停止工作"));
|
||
|
||
// 触发工作信号回调
|
||
if (m_workSignalCallback) {
|
||
m_workSignalCallback(workSignal);
|
||
}
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnWriteRegisters(uint8_t unitId, uint16_t startAddress,
|
||
uint16_t quantity, const uint16_t* values)
|
||
{
|
||
LOG_DEBUG("RobotProtocol::OnWriteRegisters - 单元ID:%d, 起始地址:%d, 数量:%d\n",
|
||
unitId, startAddress, quantity);
|
||
|
||
// 处理坐标寄存器更新
|
||
if (startAddress >= COORD_X_ADDR && startAddress < STATUS_ADDR) {
|
||
RobotCoordinate newCoordinate = m_robotCoordinate;
|
||
|
||
for (uint16_t i = 0; i < quantity; i++) {
|
||
uint16_t addr = startAddress + i;
|
||
uint16_t value = values[i];
|
||
|
||
// 根据地址更新相应的坐标分量
|
||
if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.x) + (addr - COORD_X_ADDR), &value, sizeof(uint16_t));
|
||
} else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.y) + (addr - COORD_Y_ADDR), &value, sizeof(uint16_t));
|
||
} else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.z) + (addr - COORD_Z_ADDR), &value, sizeof(uint16_t));
|
||
} else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rx) + (addr - COORD_RX_ADDR), &value, sizeof(uint16_t));
|
||
} else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.ry) + (addr - COORD_RY_ADDR), &value, sizeof(uint16_t));
|
||
} else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
|
||
memcpy(reinterpret_cast<uint16_t*>(&newCoordinate.rz) + (addr - COORD_RZ_ADDR), &value, sizeof(uint16_t));
|
||
}
|
||
}
|
||
|
||
// 更新坐标
|
||
SetRobotCoordinate(newCoordinate);
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|
||
|
||
IYModbusTCPServer::ErrorCode RobotProtocol::OnReadHoldingRegisters(uint8_t unitId, uint16_t startAddress,
|
||
uint16_t quantity, uint16_t* values)
|
||
{
|
||
LOG_DEBUG("RobotProtocol::OnReadHoldingRegisters - 单元ID:%d, 起始地址:%d, 数量:%d\n",
|
||
unitId, startAddress, quantity);
|
||
|
||
for (uint16_t i = 0; i < quantity; i++) {
|
||
uint16_t addr = startAddress + i;
|
||
|
||
if (addr >= COORD_X_ADDR && addr < COORD_X_ADDR + 2) {
|
||
// X坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.x) + (addr - COORD_X_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_Y_ADDR && addr < COORD_Y_ADDR + 2) {
|
||
// Y坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.y) + (addr - COORD_Y_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_Z_ADDR && addr < COORD_Z_ADDR + 2) {
|
||
// Z坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.z) + (addr - COORD_Z_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RX_ADDR && addr < COORD_RX_ADDR + 2) {
|
||
// RX坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rx) + (addr - COORD_RX_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RY_ADDR && addr < COORD_RY_ADDR + 2) {
|
||
// RY坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.ry) + (addr - COORD_RY_ADDR), sizeof(uint16_t));
|
||
} else if (addr >= COORD_RZ_ADDR && addr < COORD_RZ_ADDR + 2) {
|
||
// RZ坐标
|
||
memcpy(&values[i], reinterpret_cast<const uint16_t*>(&m_robotCoordinate.rz) + (addr - COORD_RZ_ADDR), sizeof(uint16_t));
|
||
} else if (addr == STATUS_ADDR) {
|
||
// 状态寄存器
|
||
values[i] = static_cast<uint16_t>(m_robotStatus);
|
||
} else {
|
||
// 未定义的地址，返回0
|
||
values[i] = 0;
|
||
}
|
||
}
|
||
|
||
return IYModbusTCPServer::ErrorCode::SUCCESS;
|
||
}
|