773 lines
23 KiB
C++
773 lines
23 KiB
C++
#include "DemoControlPresenter.h"
|
||
#include <QDebug>
|
||
#include <chrono>
|
||
#include <thread>
|
||
|
||
DemoControlPresenter::DemoControlPresenter(QObject *parent)
|
||
: QObject(parent)
|
||
{
|
||
m_robot = std::make_unique<FairinoRobotWrapper>();
|
||
}
|
||
|
||
DemoControlPresenter::~DemoControlPresenter()
|
||
{
|
||
StopWorkflow();
|
||
DisconnectController();
|
||
DisconnectRobot();
|
||
}
|
||
|
||
int DemoControlPresenter::Init()
|
||
{
|
||
SendStatusMessage("主控程序初始化完成");
|
||
return 0;
|
||
}
|
||
|
||
bool DemoControlPresenter::ConnectToController(const QString& ip, int port)
|
||
{
|
||
if (m_controllerConnected) {
|
||
SendStatusMessage("控制器已连接");
|
||
return true;
|
||
}
|
||
|
||
SendStatusMessage(QString("正在连接控制器 %1:%2...").arg(ip).arg(port));
|
||
|
||
// 创建ModbusTCP客户端
|
||
if (!IYModbusTCPClient::CreateInstance(&m_modbusClient, ip.toStdString(), port)) {
|
||
SendStatusMessage("创建ModbusTCP客户端失败");
|
||
emit controllerConnectionChanged(false);
|
||
return false;
|
||
}
|
||
|
||
// 设置连接状态回调
|
||
m_modbusClient->SetConnectionStateCallback([this](IYModbusTCPClient::ConnectionState oldState,
|
||
IYModbusTCPClient::ConnectionState newState,
|
||
const std::string& message) {
|
||
QString msg = QString::fromStdString(message);
|
||
SendStatusMessage(QString("控制器连接状态: %1").arg(msg));
|
||
|
||
bool connected = (newState == IYModbusTCPClient::CONNECTED);
|
||
m_controllerConnected = connected;
|
||
emit controllerConnectionChanged(connected);
|
||
});
|
||
|
||
// 连接
|
||
auto result = m_modbusClient->Connect();
|
||
if (result != IYModbusTCPClient::SUCCESS) {
|
||
SendStatusMessage(QString("连接控制器失败: %1")
|
||
.arg(QString::fromStdString(m_modbusClient->GetLastError())));
|
||
emit controllerConnectionChanged(false);
|
||
return false;
|
||
}
|
||
|
||
m_controllerConnected = true;
|
||
SendStatusMessage("控制器连接成功");
|
||
emit controllerConnectionChanged(true);
|
||
return true;
|
||
}
|
||
|
||
void DemoControlPresenter::DisconnectController()
|
||
{
|
||
if (m_modbusClient) {
|
||
m_modbusClient->Disconnect();
|
||
delete m_modbusClient;
|
||
m_modbusClient = nullptr;
|
||
}
|
||
m_controllerConnected = false;
|
||
emit controllerConnectionChanged(false);
|
||
SendStatusMessage("控制器已断开连接");
|
||
}
|
||
|
||
bool DemoControlPresenter::ConnectToRobot(const QString& ip)
|
||
{
|
||
if (m_robotConnected) {
|
||
SendStatusMessage("机械臂已连接");
|
||
return true;
|
||
}
|
||
|
||
SendStatusMessage(QString("正在连接机械臂 %1...").arg(ip));
|
||
|
||
errno_t ret = m_robot->Connect(ip.toStdString());
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("连接机械臂失败: 错误码 %1").arg(ret));
|
||
emit robotConnectionChanged(false);
|
||
return false;
|
||
}
|
||
|
||
m_robotConnected = true;
|
||
SendStatusMessage("机械臂连接成功");
|
||
emit robotConnectionChanged(true);
|
||
return true;
|
||
}
|
||
|
||
void DemoControlPresenter::DisconnectRobot()
|
||
{
|
||
if (m_robot) {
|
||
m_robot->Disconnect();
|
||
}
|
||
m_robotConnected = false;
|
||
emit robotConnectionChanged(false);
|
||
SendStatusMessage("机械臂已断开连接");
|
||
}
|
||
|
||
bool DemoControlPresenter::IsControllerConnected() const
|
||
{
|
||
return m_controllerConnected;
|
||
}
|
||
|
||
bool DemoControlPresenter::IsRobotConnected() const
|
||
{
|
||
return m_robotConnected;
|
||
}
|
||
|
||
void DemoControlPresenter::StartWorkflow()
|
||
{
|
||
if (m_workflowRunning) {
|
||
SendStatusMessage("工作流程已在运行中");
|
||
return;
|
||
}
|
||
|
||
if (!m_controllerConnected) {
|
||
SendStatusMessage("错误: 控制器未连接");
|
||
return;
|
||
}
|
||
|
||
if (!m_robotConnected) {
|
||
SendStatusMessage("错误: 机械臂未连接");
|
||
return;
|
||
}
|
||
|
||
// 清理旧的线程对象（如果存在）
|
||
if (m_workflowThread.joinable()) {
|
||
m_workflowThread.join();
|
||
}
|
||
|
||
m_workflowShouldStop = false;
|
||
m_workflowPaused = false;
|
||
m_workflowStepMode = false;
|
||
m_nextStepToExecute = 0; // 从第一步开始
|
||
m_workflowRunning = true;
|
||
|
||
// 启动工作流程线程
|
||
m_workflowThread = std::thread(&DemoControlPresenter::WorkflowThreadFunc, this);
|
||
|
||
SendStatusMessage("工作流程已启动");
|
||
}
|
||
|
||
void DemoControlPresenter::StopWorkflow()
|
||
{
|
||
if (!m_workflowRunning) {
|
||
return;
|
||
}
|
||
|
||
m_workflowShouldStop = true;
|
||
m_workflowPaused = false;
|
||
m_workflowStepMode = false;
|
||
m_nextStepToExecute = 0; // 重置步骤
|
||
|
||
// 唤醒可能在等待的线程
|
||
m_stepCondition.notify_all();
|
||
|
||
if (m_workflowThread.joinable()) {
|
||
m_workflowThread.join();
|
||
}
|
||
|
||
m_workflowRunning = false;
|
||
UpdateWorkflowStep(WorkflowStep::Idle, "工作流程已停止");
|
||
}
|
||
|
||
void DemoControlPresenter::PauseWorkflow()
|
||
{
|
||
if (!m_workflowRunning || m_workflowPaused) {
|
||
return;
|
||
}
|
||
|
||
m_workflowPaused = true;
|
||
SendStatusMessage("工作流程已暂停");
|
||
}
|
||
|
||
void DemoControlPresenter::ResumeWorkflow()
|
||
{
|
||
if (!m_workflowRunning || !m_workflowPaused) {
|
||
return;
|
||
}
|
||
|
||
m_workflowPaused = false;
|
||
m_workflowStepMode = false;
|
||
m_stepCondition.notify_all();
|
||
SendStatusMessage("工作流程已恢复");
|
||
}
|
||
|
||
void DemoControlPresenter::StepWorkflow()
|
||
{
|
||
if (m_workflowRunning && !m_workflowPaused) {
|
||
SendStatusMessage("请先暂停工作流程");
|
||
return;
|
||
}
|
||
|
||
if (!m_controllerConnected) {
|
||
SendStatusMessage("错误: 控制器未连接");
|
||
return;
|
||
}
|
||
|
||
if (!m_robotConnected) {
|
||
SendStatusMessage("错误: 机械臂未连接");
|
||
return;
|
||
}
|
||
|
||
// 如果工作流程未运行，启动单步模式
|
||
if (!m_workflowRunning) {
|
||
// 清理旧的线程对象（如果存在）
|
||
if (m_workflowThread.joinable()) {
|
||
m_workflowThread.join();
|
||
}
|
||
|
||
m_workflowShouldStop = false;
|
||
m_workflowPaused = true;
|
||
m_workflowStepMode = true;
|
||
m_workflowStepReady = true;
|
||
m_nextStepToExecute = 0; // 从第一步开始
|
||
m_workflowRunning = true;
|
||
|
||
// 启动工作流程线程
|
||
m_workflowThread = std::thread(&DemoControlPresenter::WorkflowThreadFunc, this);
|
||
SendStatusMessage("单步执行已启动，从第一步开始");
|
||
} else {
|
||
// 工作流程已运行且暂停，切换到单步模式并执行下一步
|
||
m_workflowStepMode = true; // 切换到单步模式
|
||
m_workflowStepReady = true;
|
||
m_stepCondition.notify_all();
|
||
SendStatusMessage(QString("单步执行下一步（步骤 %1）").arg(m_nextStepToExecute + 1));
|
||
}
|
||
}
|
||
|
||
void DemoControlPresenter::SetReadyPosition(double x, double y, double z, double rx, double ry, double rz)
|
||
{
|
||
m_readyPosition.x = x;
|
||
m_readyPosition.y = y;
|
||
m_readyPosition.z = z;
|
||
m_readyPosition.rx = rx;
|
||
m_readyPosition.ry = ry;
|
||
m_readyPosition.rz = rz;
|
||
}
|
||
|
||
void DemoControlPresenter::SetPlacePosition(double x, double y, double z, double rx, double ry, double rz)
|
||
{
|
||
m_placePosition.x = x;
|
||
m_placePosition.y = y;
|
||
m_placePosition.z = z;
|
||
m_placePosition.rx = rx;
|
||
m_placePosition.ry = ry;
|
||
m_placePosition.rz = rz;
|
||
}
|
||
|
||
void DemoControlPresenter::SetGraspOffset(double offsetZ)
|
||
{
|
||
m_graspOffsetZ = offsetZ;
|
||
}
|
||
|
||
void DemoControlPresenter::SetToolNumber(int tool)
|
||
{
|
||
m_toolNumber = tool;
|
||
}
|
||
|
||
void DemoControlPresenter::SetVelocity(double velocity)
|
||
{
|
||
m_velocity = velocity;
|
||
}
|
||
|
||
void DemoControlPresenter::SetAcceleration(double acceleration)
|
||
{
|
||
m_acceleration = acceleration;
|
||
}
|
||
|
||
bool DemoControlPresenter::TestMoveToReady()
|
||
{
|
||
if (!m_robotConnected) {
|
||
SendStatusMessage("错误: 机械臂未连接");
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("测试移动到准备位置...");
|
||
|
||
errno_t ret = m_robot->MoveJByPose(
|
||
m_readyPosition.x, m_readyPosition.y, m_readyPosition.z,
|
||
m_readyPosition.rx, m_readyPosition.ry, m_readyPosition.rz,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("移动到准备位置失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达准备位置");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::TestMoveToPlace()
|
||
{
|
||
if (!m_robotConnected) {
|
||
SendStatusMessage("错误: 机械臂未连接");
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("测试移动到放置位置...");
|
||
|
||
errno_t ret = m_robot->MoveJByPose(
|
||
m_placePosition.x, m_placePosition.y, m_placePosition.z,
|
||
m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("移动到放置位置失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达放置位置");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::GetCurrentPosition(double& x, double& y, double& z, double& rx, double& ry, double& rz)
|
||
{
|
||
if (!m_robotConnected) {
|
||
SendStatusMessage("错误: 机械臂未连接");
|
||
return false;
|
||
}
|
||
|
||
errno_t ret = m_robot->GetCurrentTCPPose(x, y, z, rx, ry, rz, 0);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("读取当前位置失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage(QString("当前位置: X=%1, Y=%2, Z=%3, RX=%4, RY=%5, RZ=%6")
|
||
.arg(x, 0, 'f', 2).arg(y, 0, 'f', 2).arg(z, 0, 'f', 2)
|
||
.arg(rx, 0, 'f', 2).arg(ry, 0, 'f', 2).arg(rz, 0, 'f', 2));
|
||
return true;
|
||
}
|
||
|
||
void DemoControlPresenter::WorkflowThreadFunc()
|
||
{
|
||
while (!m_workflowShouldStop) {
|
||
// 检查是否需要等待（暂停或单步模式）
|
||
if (m_workflowPaused || m_workflowStepMode) {
|
||
std::unique_lock<std::mutex> lock(m_stepMutex);
|
||
m_stepCondition.wait(lock, [this]() {
|
||
return m_workflowShouldStop || (!m_workflowPaused && !m_workflowStepMode) || m_workflowStepReady;
|
||
});
|
||
|
||
if (m_workflowShouldStop) break;
|
||
|
||
// 单步模式下，执行完一步后重新等待
|
||
if (m_workflowStepMode) {
|
||
m_workflowStepReady = false;
|
||
}
|
||
}
|
||
|
||
// 根据 m_nextStepToExecute 执行对应的步骤
|
||
bool stepSuccess = false;
|
||
int currentStep = m_nextStepToExecute.load();
|
||
|
||
switch (currentStep) {
|
||
case 0: // 步骤1: 机械臂运行到准备位置
|
||
stepSuccess = ExecuteMovingToReady();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "移动到准备位置失败");
|
||
} else {
|
||
m_nextStepToExecute = 1;
|
||
}
|
||
break;
|
||
|
||
case 1: // 步骤2: 触发控制器检测
|
||
stepSuccess = ExecuteTriggeringDetection();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "触发检测失败");
|
||
} else {
|
||
m_nextStepToExecute = 2;
|
||
}
|
||
break;
|
||
|
||
case 2: // 步骤3: 等待检测完成
|
||
stepSuccess = ExecuteWaitingDetectionComplete();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "等待检测完成失败");
|
||
} else {
|
||
m_nextStepToExecute = 3;
|
||
}
|
||
break;
|
||
|
||
case 3: // 步骤4: 读取检测结果
|
||
stepSuccess = ExecuteReadingResult();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "读取检测结果失败");
|
||
} else {
|
||
m_nextStepToExecute = 4;
|
||
}
|
||
break;
|
||
|
||
case 4: // 步骤5: 机械臂移动到抓取位置
|
||
stepSuccess = ExecuteMovingToGrasp();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "移动到抓取位置失败");
|
||
} else {
|
||
m_nextStepToExecute = 5;
|
||
}
|
||
break;
|
||
|
||
case 5: // 步骤6: 执行抓取
|
||
stepSuccess = ExecuteGrasping();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "执行抓取失败");
|
||
} else {
|
||
m_nextStepToExecute = 6;
|
||
}
|
||
break;
|
||
|
||
case 6: // 步骤7: 机械臂移动到放置位置
|
||
stepSuccess = ExecuteMovingToPlace();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "移动到放置位置失败");
|
||
} else {
|
||
m_nextStepToExecute = 7;
|
||
}
|
||
break;
|
||
|
||
case 7: // 步骤8: 执行放置
|
||
stepSuccess = ExecutePlacing();
|
||
if (!stepSuccess) {
|
||
UpdateWorkflowStep(WorkflowStep::Error, "执行放置失败");
|
||
} else {
|
||
// 完成一次循环
|
||
UpdateWorkflowStep(WorkflowStep::Completed, "工作流程完成");
|
||
|
||
// 如果是单步模式，完成后停止工作流程
|
||
if (m_workflowStepMode) {
|
||
m_nextStepToExecute = 0; // 重置到第一步
|
||
m_workflowRunning = false;
|
||
SendStatusMessage("单步执行已完成所有步骤，下次将从第一步重新开始");
|
||
return; // 退出线程
|
||
}
|
||
|
||
// 连续模式下，重置到第一步继续循环
|
||
m_nextStepToExecute = 0;
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||
}
|
||
break;
|
||
|
||
default:
|
||
// 未知步骤，重置
|
||
m_nextStepToExecute = 0;
|
||
break;
|
||
}
|
||
|
||
// 如果步骤失败，退出循环
|
||
if (!stepSuccess) {
|
||
break;
|
||
}
|
||
|
||
// 如果是单步模式，执行完一步后继续等待
|
||
if (m_workflowStepMode && currentStep < 7) {
|
||
continue;
|
||
}
|
||
|
||
// 检查是否需要停止
|
||
if (m_workflowShouldStop) break;
|
||
|
||
// 检查暂停
|
||
if (m_workflowPaused) continue;
|
||
}
|
||
|
||
m_workflowRunning = false;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteMovingToReady()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::MovingToReady);
|
||
|
||
errno_t ret = m_robot->MoveJByPose(
|
||
m_readyPosition.x, m_readyPosition.y, m_readyPosition.z,
|
||
m_readyPosition.rx, m_readyPosition.ry, m_readyPosition.rz,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("机械臂移动失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("机械臂已到达准备位置");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteTriggeringDetection()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::TriggeringDetection);
|
||
|
||
// 写1到地址0触发检测
|
||
if (!WriteModbusRegister(0, 1)) {
|
||
SendStatusMessage("触发检测失败");
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已触发控制器检测");
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteWaitingDetectionComplete()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::WaitingDetectionComplete);
|
||
|
||
// 轮询地址1，等待检测完成
|
||
const int maxRetries = 100; // 最多等待10秒
|
||
for (int i = 0; i < maxRetries && !m_workflowShouldStop; ++i) {
|
||
uint16_t status = 0;
|
||
if (!ReadModbusRegister(1, status)) {
|
||
SendStatusMessage("读取检测状态失败");
|
||
return false;
|
||
}
|
||
|
||
if (status == 1) {
|
||
SendStatusMessage("检测完成（正常）");
|
||
return true;
|
||
} else if (status == 2) {
|
||
SendStatusMessage("检测完成（异常）");
|
||
return false;
|
||
}
|
||
|
||
// 状态为0，继续等待
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||
}
|
||
|
||
SendStatusMessage("等待检测完成超时");
|
||
return false;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteReadingResult()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::ReadingResult);
|
||
|
||
// 读取地址2-13的位姿数据（6个float = 12个寄存器）
|
||
std::vector<uint16_t> registers;
|
||
if (!ReadModbusRegisters(2, 12, registers)) {
|
||
SendStatusMessage("读取检测结果失败");
|
||
return false;
|
||
}
|
||
|
||
// 解析float数据
|
||
DetectionResult result;
|
||
result.x = IYModbusTCPClient::RegistersToFloat(registers[0], registers[1]);
|
||
result.y = IYModbusTCPClient::RegistersToFloat(registers[2], registers[3]);
|
||
result.z = IYModbusTCPClient::RegistersToFloat(registers[4], registers[5]);
|
||
result.roll = IYModbusTCPClient::RegistersToFloat(registers[6], registers[7]);
|
||
result.pitch = IYModbusTCPClient::RegistersToFloat(registers[8], registers[9]);
|
||
result.yaw = IYModbusTCPClient::RegistersToFloat(registers[10], registers[11]);
|
||
result.valid = true;
|
||
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_resultMutex);
|
||
m_lastResult = result;
|
||
}
|
||
|
||
emit detectionResultUpdated(result);
|
||
|
||
SendStatusMessage(QString("检测结果: X=%1, Y=%2, Z=%3, Roll=%4, Pitch=%5, Yaw=%6")
|
||
.arg(result.x, 0, 'f', 2).arg(result.y, 0, 'f', 2).arg(result.z, 0, 'f', 2)
|
||
.arg(result.roll, 0, 'f', 2).arg(result.pitch, 0, 'f', 2).arg(result.yaw, 0, 'f', 2));
|
||
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteMovingToGrasp()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::MovingToGrasp);
|
||
|
||
DetectionResult result;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_resultMutex);
|
||
result = m_lastResult;
|
||
}
|
||
|
||
if (!result.valid) {
|
||
SendStatusMessage("检测结果无效");
|
||
return false;
|
||
}
|
||
|
||
// 第一步：移动到目标位置上方（安全高度）
|
||
SendStatusMessage("移动到抓取位置上方...");
|
||
errno_t ret = m_robot->MoveJByPose(
|
||
result.x, result.y, result.z,
|
||
result.roll, result.pitch, result.yaw,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("移动到抓取位置上方失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达抓取位置上方，准备下降...");
|
||
|
||
// 第二步：下降到抓取位置（加上Z轴偏移）
|
||
ret = m_robot->MoveJByPose(
|
||
result.x, result.y, result.z + m_graspOffsetZ,
|
||
result.roll, result.pitch, result.yaw,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("下降到抓取位置失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达抓取位置");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteGrasping()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::Grasping);
|
||
|
||
// 这里可以添加夹爪控制逻辑
|
||
// 目前仅模拟等待
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||
|
||
SendStatusMessage("抓取完成");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecuteMovingToPlace()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::MovingToPlace);
|
||
|
||
DetectionResult result;
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_resultMutex);
|
||
result = m_lastResult;
|
||
}
|
||
|
||
if (!result.valid) {
|
||
SendStatusMessage("检测结果无效");
|
||
return false;
|
||
}
|
||
|
||
// 第一步：从抓取位置抬起到安全高度
|
||
SendStatusMessage("从抓取位置抬起...");
|
||
errno_t ret = m_robot->MoveJByPose(
|
||
result.x, result.y, result.z,
|
||
result.roll, result.pitch, result.yaw,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("抬起失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已抬起到安全高度，准备移动到放置位置上方...");
|
||
|
||
// 第二步：移动到放置位置上方（安全高度）
|
||
ret = m_robot->MoveJByPose(
|
||
m_placePosition.x, m_placePosition.y, m_placePosition.z - m_graspOffsetZ,
|
||
m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("移动到放置位置上方失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达放置位置上方，准备下降...");
|
||
|
||
// 第三步：下降到放置位置
|
||
ret = m_robot->MoveJByPose(
|
||
m_placePosition.x, m_placePosition.y, m_placePosition.z,
|
||
m_placePosition.rx, m_placePosition.ry, m_placePosition.rz,
|
||
static_cast<float>(m_velocity), static_cast<float>(m_acceleration), -1.0f,
|
||
m_toolNumber, 0
|
||
);
|
||
|
||
if (ret != 0) {
|
||
SendStatusMessage(QString("下降到放置位置失败: 错误码 %1").arg(ret));
|
||
return false;
|
||
}
|
||
|
||
SendStatusMessage("已到达放置位置");
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::ExecutePlacing()
|
||
{
|
||
UpdateWorkflowStep(WorkflowStep::Placing);
|
||
|
||
// 这里可以添加夹爪释放逻辑
|
||
// 目前仅模拟等待
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||
|
||
SendStatusMessage("放置完成");
|
||
return true;
|
||
}
|
||
|
||
void DemoControlPresenter::UpdateWorkflowStep(WorkflowStep step, const QString& message)
|
||
{
|
||
{
|
||
std::lock_guard<std::mutex> lock(m_stepMutex);
|
||
m_currentStep = step;
|
||
}
|
||
|
||
QString msg = message.isEmpty() ? WorkflowStepToString(step) : message;
|
||
emit workflowStepChanged(step, msg);
|
||
SendStatusMessage(msg);
|
||
}
|
||
|
||
void DemoControlPresenter::SendStatusMessage(const QString& message)
|
||
{
|
||
emit statusMessageUpdated(message);
|
||
}
|
||
|
||
bool DemoControlPresenter::ReadModbusRegister(int address, uint16_t& value)
|
||
{
|
||
if (!m_modbusClient || !m_controllerConnected) {
|
||
return false;
|
||
}
|
||
|
||
std::vector<uint16_t> values;
|
||
auto result = m_modbusClient->ReadHoldingRegisters(address, 1, values);
|
||
if (result != IYModbusTCPClient::SUCCESS || values.empty()) {
|
||
return false;
|
||
}
|
||
|
||
value = values[0];
|
||
return true;
|
||
}
|
||
|
||
bool DemoControlPresenter::WriteModbusRegister(int address, uint16_t value)
|
||
{
|
||
if (!m_modbusClient || !m_controllerConnected) {
|
||
return false;
|
||
}
|
||
|
||
auto result = m_modbusClient->WriteSingleRegister(address, value);
|
||
return (result == IYModbusTCPClient::SUCCESS);
|
||
}
|
||
|
||
bool DemoControlPresenter::ReadModbusRegisters(int startAddress, int quantity, std::vector<uint16_t>& values)
|
||
{
|
||
if (!m_modbusClient || !m_controllerConnected) {
|
||
return false;
|
||
}
|
||
|
||
auto result = m_modbusClient->ReadHoldingRegisters(startAddress, quantity, values);
|
||
return (result == IYModbusTCPClient::SUCCESS);
|
||
}
|