From f6138dd2edf25fd06954ff38a49f9ba20ac10d89 Mon Sep 17 00:00:00 2001 From: tangchao0503 <735056338@qq.com> Date: Mon, 14 Apr 2025 10:02:37 +0800 Subject: [PATCH] =?UTF-8?q?QTcpSocket=E5=BC=82=E6=AD=A5=E9=80=9A=E8=AE=AF?= =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9C=BA=E6=A2=B0=E8=87=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- HPPA/HPPA.cpp | 10 +- HPPA/HPPA.h | 1 + HPPA/HPPA.ui | 8 +- HPPA/HPPA.vcxproj | 5 +- HPPA/HPPA.vcxproj.filters | 9 + HPPA/RobotArmControl.cpp | 427 ++++++++++++++++++++++++++++++++++++++ HPPA/RobotArmControl.h | 152 ++++++++++++++ HPPA/RobotArmControl.ui | 138 ++++++++++++ 8 files changed, 744 insertions(+), 6 deletions(-) create mode 100644 HPPA/RobotArmControl.cpp create mode 100644 HPPA/RobotArmControl.h create mode 100644 HPPA/RobotArmControl.ui diff --git a/HPPA/HPPA.cpp b/HPPA/HPPA.cpp index 19b0aa8..67b7fa5 100644 --- a/HPPA/HPPA.cpp +++ b/HPPA/HPPA.cpp @@ -292,6 +292,14 @@ HPPA::HPPA(QWidget *parent) connect(pc, &PowerControl::powerOpened, this, &HPPA::newMotor); connect(pc, &PowerControl::powerClosed, this, &HPPA::deleteMotor); + //机械臂控制 + RobotArmControl* rac = new RobotArmControl(); + QDockWidget* dock_rac = new QDockWidget(QString::fromLocal8Bit("机械臂控制"), this); + dock_rac->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea); + dock_rac->setWidget(rac); + tabifyDockWidget(dock_adt, dock_rac); + mPanelMenu->addAction(dock_rac->toggleViewAction()); + createActionGroups(); connect(mImagerGroup, &QActionGroup::triggered, this, &HPPA::selectingImager); @@ -796,7 +804,7 @@ void HPPA::timerEvent(QTimerEvent *event) { if (m_xMotor == nullptr || m_yMotor == nullptr) { - qDebug() << "Motor pointer is null!!!!!"; + //qDebug() << "Motor pointer is null!!!!!"; xmotor_state_label1->setStyleSheet("QLabel{background-color:rgb(255,0,0);}"); SetXMotorWidgetEnable(false); diff --git a/HPPA/HPPA.h b/HPPA/HPPA.h index 82554f7..3f6d56b 100644 --- a/HPPA/HPPA.h +++ b/HPPA/HPPA.h @@ -33,6 +33,7 @@ #include "adjustTable.h" #include "PowerControl.h" #include "PathPlan.h" +#include "RobotArmControl.h" #include "hppaConfigFile.h" #include "path_tc.h" diff --git a/HPPA/HPPA.ui b/HPPA/HPPA.ui index f245af2..341a2e3 100644 --- a/HPPA/HPPA.ui +++ b/HPPA/HPPA.ui @@ -6,8 +6,8 @@ 0 0 - 1120 - 675 + 1318 + 882 @@ -46,7 +46,7 @@ - 1 + 0 @@ -84,7 +84,7 @@ 0 0 - 1120 + 1318 30 diff --git a/HPPA/HPPA.vcxproj b/HPPA/HPPA.vcxproj index 4c3f7cb..c70716c 100644 --- a/HPPA/HPPA.vcxproj +++ b/HPPA/HPPA.vcxproj @@ -86,7 +86,7 @@ MultiThreadedDebugDLL - Windows + Console true @@ -116,6 +116,7 @@ + Create Create @@ -139,6 +140,7 @@ + @@ -153,6 +155,7 @@ + diff --git a/HPPA/HPPA.vcxproj.filters b/HPPA/HPPA.vcxproj.filters index 64429d0..1b7ece5 100644 --- a/HPPA/HPPA.vcxproj.filters +++ b/HPPA/HPPA.vcxproj.filters @@ -106,6 +106,9 @@ Source Files + + Source Files + @@ -153,6 +156,9 @@ Header Files + + Header Files + @@ -196,6 +202,9 @@ Form Files + + Form Files + diff --git a/HPPA/RobotArmControl.cpp b/HPPA/RobotArmControl.cpp new file mode 100644 index 0000000..85d5689 --- /dev/null +++ b/HPPA/RobotArmControl.cpp @@ -0,0 +1,427 @@ +#include "RobotArmControl.h" + +RobotArmControl::RobotArmControl(QWidget* parent): QDialog(parent) +{ + ui.setupUi(this); + robotController = new RobotController(this); + connect(robotController, SIGNAL(commandResponse(bool, const QJsonObject)), this, SLOT(onCommandResponse(bool, const QJsonObject))); + + + connect(ui.get_task_list_btn, SIGNAL(clicked()), this, SLOT(getTaskList())); + connect(ui.get_pose_btn, SIGNAL(clicked()), this, SLOT(getPose())); + connect(ui.connect2arm_btn, SIGNAL(clicked()), this, SLOT(connectRobotArm())); + + connect(ui.execute_task_btn, SIGNAL(clicked()), this, SLOT(executeTask())); + connect(ui.pause_task_btn, SIGNAL(clicked()), this, SLOT(pauseTask())); + connect(ui.continue_task_btn, SIGNAL(clicked()), this, SLOT(continueTask())); + + robotMonitor = new EC8056; + connect(robotMonitor, &EC8056::dataReceived, this, &RobotArmControl::monitorRobotArm); + + + m_pModel = new QStringListModel(ui.taskList_listView); + + //将字符串列表模型设置到树形视图上 + ui.taskList_listView->setModel(m_pModel); +} + +RobotArmControl::~RobotArmControl() +{ + +} + +void RobotArmControl::monitorRobotArm(const ECData& data) +{ + double x = data.machinePose[0]; + double y = data.machinePose[1]; + double z = data.machinePose[2]; + + ui.pose_x_label->setText(QString::number(x)); + ui.pose_y_label->setText(QString::number(y)); + ui.pose_z_label->setText(QString::number(z)); +} + +void RobotArmControl::onCommandResponse(bool success, const QJsonObject& response) +{ + if (success) + { + QString re; + if (response.contains("result")) + { + re = response["result"].toVariant().toString(); + ui.textEdit->append("Result: " + re); + } + else if (response.contains("error")) + { + auto errorStr = response["error"].toObject()["message"].toString(); + + ui.textEdit->append("Error: " + errorStr); + } + } +} + +void RobotArmControl::getTaskList() +{ + QString pythonScript = "D:\\PycharmProjects\\ELITE_ROBOTS\\get_jbi_filename.py"; + + QProcess process; + + process.start("C:\\ProgramData\\anaconda3\\python.exe", QStringList() << pythonScript); + process.waitForFinished(); + QString output = process.readAllStandardOutput(); + + QStringList files; + //files.append("tc20250324down.jbi"); + //files.append("tc20250324circle.jbi"); + //files.append("tc20250324side.jbi"); + QStringList lines = output.split('\n', QString::SkipEmptyParts); + for (const QString& line : lines) + { + files.append(line.trimmed()); + } + + ////先清除数据，然后添加数据 + //for (size_t i = 0; i < files.length(); i++) + //{ + // int row = m_pModel->rowCount(); + // m_pModel->insertRow(row); + // QModelIndex index = m_pModel->index(row); + // m_pModel->setData(index, files[i]); + //} + + m_pModel->setStringList(files); +} + +void RobotArmControl::getPose() +{ + robotController->getRobotPose(); +} + +void RobotArmControl::connectRobotArm() +{ + bool re = robotController->connectToRobot("192.168.1.100"); + robotMonitor->connectToHost("192.168.1.100"); +} + +void RobotArmControl::executeTask() +{ + QModelIndex index = ui.taskList_listView->currentIndex(); + if (-1 == index.row()) + { + //弹窗提示用户选择文件 + ui.textEdit->append("Please select file on the left!"); + return; + } + QString fileName = index.data(Qt::DisplayRole).toString(); + + robotController->runJbi(fileName); +} + +void RobotArmControl::pauseTask() +{ + robotController->pauseTask(); +} + +void RobotArmControl::continueTask() +{ + robotController->continueTask(); +} + + + + + + +RobotController::RobotController(QObject* parent) : QObject(parent), socket(new QTcpSocket(this)) +{ + connect(socket, &QTcpSocket::readyRead, this, &RobotController::onReadyRead); + + timer = new QTimer(this); + connect(timer, SIGNAL(timeout()), this, SLOT(getPoint())); +} + +RobotController::~RobotController() +{ + disconnectFromRobot(); +} + +bool RobotController::connectToRobot(const QString& ip, quint16 port) +{ + socket->setProxy(QNetworkProxy::NoProxy); + + socket->connectToHost(ip, port); + + if (!socket->waitForConnected(3000)) { + qDebug() << "Connection failed:" << socket->errorString(); + return false; + } + qDebug() << "Connected successfully!"; + + return true; + +} + +void RobotController::disconnectFromRobot() +{ + if (socket->isOpen()) + { + socket->close(); + } +} + +void RobotController::getPoint() +{ + getJbiState(); + getCurrentJobLine(); +} + +void RobotController::sendCommand(const QString& cmd, const QJsonValue& params, int id) +{ + QJsonObject request; + request["method"] = cmd; + request["params"] = params; + request["jsonrpc"] = "2.0"; + request["id"] = id; + + QJsonDocument doc(request); + QByteArray data = doc.toJson(QJsonDocument::Compact) + "\n"; + qDebug() << "send command:" << data.constData(); + + socket->write(data); +} + +void RobotController::onReadyRead() +{ + QByteArray data = socket->readAll(); + QJsonDocument doc = QJsonDocument::fromJson(data); + if (!doc.isNull() && doc.isObject()) + { + QJsonObject response = doc.object(); + //qDebug() << "Received all:" << response; + + if (response.contains("result")) + { + //qDebug() << "Received result:" << response["result"].toVariant(); + } + else if (response.contains("error")) + { + //qDebug() << "Received error:" << response["error"]; + } + + emit commandResponse(true, doc.object()); + } + else + { + emit commandResponse(false, QJsonObject()); + } +} + +void RobotController::getRobotPose() +{ + sendCommand("getRobotPose"); +} + +void RobotController::getRobotState() +{ + sendCommand("getRobotState"); +} + +void RobotController::getJbiState() +{ + sendCommand("getJbiState"); +} + +void RobotController::getCurrentJobLine() +{ + sendCommand("getCurrentJobLine"); +} + +void RobotController::getRobotMode() +{ + sendCommand("getRobotMode"); +} + +void RobotController::runJbi(const QString& jbiFilename) +{ + QJsonObject paramsRunJbi; + paramsRunJbi["filename"] = jbiFilename;//使用对象结构 + + QJsonObject params_set_servo_status; + params_set_servo_status["status"] = 1;//使用对象结构 + + //sendCommand("checkJbiExist", paramsRunJbi); + sendCommand("set_servo_status", params_set_servo_status); + sendCommand("runJbi", paramsRunJbi); + + //timer->start(1000); +} + +void RobotController::pauseTask() +{ + sendCommand("pause"); +} + +void RobotController::run() +{ + sendCommand("run"); +} + +void RobotController::continueTask() +{ + sendCommand("run"); +} + +void RobotController::setRobotPowerStatus(int status) +{ + sendCommand("set_robot_power_status", QJsonArray{ status }); +} + + + + + + + + +EC8056::EC8056(QObject* parent) : QObject(parent), socket(new QTcpSocket(this)) +{ + connect(socket, &QTcpSocket::readyRead, this, &EC8056::onReadyRead); + /*connect(socket, QOverload::of(&QTcpSocket::errorOccurred), + this, &EC8056::onSocketError);*/ + //connect(socket, SIGNAL(error(QAbstractSocket::SocketError)), + // this, SLOT(onSocketError(QAbstractSocket::SocketError))); +} + +void EC8056::connectToHost(const QString& host, quint16 port) +{ + socket->setProxy(QNetworkProxy::NoProxy); + + socket->connectToHost(QHostAddress(host), port); + + if (!socket->waitForConnected(3000)) { + qDebug() << "Connection failed:" << socket->errorString(); + return; + } + qDebug() << "Connected successfully!"; +} + +quint8 EC8056::readUInt8(const QByteArray& buf, int& offset) +{ + return static_cast(buf[offset++]); +} + +quint32 EC8056::readUInt32(const QByteArray& buf, int& offset) +{ + quint32 val; + memcpy(&val, buf.constData() + offset, sizeof(val)); + offset += sizeof(val); + return qFromBigEndian(val); +} + +qint32 EC8056::readInt32(const QByteArray& buf, int& offset) +{ + qint32 val; + memcpy(&val, buf.constData() + offset, sizeof(val)); + offset += sizeof(val); + return qFromBigEndian(val); +} + +quint64 EC8056::readUInt64(const QByteArray& buf, int& offset) +{ + quint64 val; + memcpy(&val, buf.constData() + offset, sizeof(val)); + offset += sizeof(val); + return qFromBigEndian(val); +} + +double EC8056::readDouble(const QByteArray& buf, int& offset) +{ + QByteArray b = buf.mid(offset, sizeof(double)); + offset += sizeof(double); + if (QSysInfo::ByteOrder == QSysInfo::LittleEndian) + std::reverse(b.begin(), b.end()); + + double val; + memcpy(&val, b.constData(), sizeof(double)); + return val; +} + +void EC8056::onReadyRead() +{ + buffer.append(socket->readAll()); + + if (buffer.size() < 1024) + return; + + int offset = 0; + ECData data; + + data.msgSize = readUInt32(buffer, offset); + offset = 1020; + data.matchingWord = readUInt32(buffer, offset); + + if ((data.msgSize == 1024)/* && (data.matchingWord == 3967833836)*/) + { + offset = 0; + + data.msgSize = readUInt32(buffer, offset); + data.timeStamp = readUInt64(buffer, offset); + data.auto_cycle = readUInt8(buffer, offset); + + for (int i = 0; i < 8; ++i) data.machinePos[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.machinePose[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.machineUserPose[i] = readDouble(buffer, offset); + for (int i = 0; i < 8; ++i) data.torque[i] = readDouble(buffer, offset); + + data.robotState = readUInt32(buffer, offset); + data.servoReady = readUInt32(buffer, offset); + data.can_motor_run = readUInt32(buffer, offset); + + for (int i = 0; i < 8; ++i) data.motor_speed[i] = readInt32(buffer, offset); + + data.robotMode = readUInt32(buffer, offset); + + for (int i = 0; i < 3; ++i) data.analog_ioInput[i] = readDouble(buffer, offset); + for (int i = 0; i < 5; ++i) data.analog_ioOutput[i] = readDouble(buffer, offset); + + data.digital_ioInput = readUInt64(buffer, offset); + data.digital_ioOutput = readUInt64(buffer, offset); + + data.collision = readUInt8(buffer, offset); + + for (int i = 0; i < 6; ++i) data.machineFlangePose[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.machineUserFlangePose[i] = readDouble(buffer, offset); + + data.emergencyStopState = readUInt8(buffer, offset); + data.tcpSpeed = readDouble(buffer, offset); + for (int i = 0; i < 8; ++i) data.joIntSpeed[i] = readDouble(buffer, offset); + data.tcpAcc = readDouble(buffer, offset); + for (int i = 0; i < 8; ++i) data.joIntAcc[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.joIntTemperature[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.joIntTorque[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.extJoIntTorques[i] = readDouble(buffer, offset); + for (int i = 0; i < 6; ++i) data.exTcpForceIntool[i] = readDouble(buffer, offset); + + data.dragState = readUInt8(buffer, offset); + data.sensor_connected_state = readUInt8(buffer, offset); + data.reserved = readUInt8(buffer, offset); + data.matchingWord = readUInt32(buffer, offset); + + /*if (data.msgSize != 1024 || data.matchingWord != 0xec8056ec) { + buffer.clear(); + emit errorOccurred("Invalid packet received"); + return; + }*/ + + emit dataReceived(data); + } + + + buffer.clear(); +} + +void EC8056::onSocketError(QAbstractSocket::SocketError socketError) +{ + Q_UNUSED(socketError) + emit errorOccurred(socket->errorString()); +} diff --git a/HPPA/RobotArmControl.h b/HPPA/RobotArmControl.h new file mode 100644 index 0000000..9548609 --- /dev/null +++ b/HPPA/RobotArmControl.h @@ -0,0 +1,152 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ui_RobotArmControl.h" + + +struct ECData +{ + quint32 msgSize; + quint64 timeStamp; + quint8 auto_cycle; + double machinePos[8];//关节坐标 + double machinePose[6];//基座坐标 + double machineUserPose[6];//用户坐标 + double torque[8];//关节额定力矩百分比 + quint32 robotState;//机器人状态 + quint32 servoReady;//伺服使能状态 + quint32 can_motor_run;//同步状态 + qint32 motor_speed[8];//各轴电机转速 + quint32 robotMode;//机器人模式 + double analog_ioInput[3];//模拟量输入口数据 + double analog_ioOutput[5];//模拟量输出口数据 + quint64 digital_ioInput;//数字量输入口数据的二进制形式 + quint64 digital_ioOutput;//数字量输出口数据的二进制形式 + quint8 collision;//碰撞报警状态 + double machineFlangePose[6];//基座坐标系下的法兰盘中心位姿 + double machineUserFlangePose[6];//用户坐标系下的法兰盘中心位姿 + quint8 emergencyStopState;//当前是否处于急停状态 + double tcpSpeed;//tcp运动速度 + double joIntSpeed[8];//关节运动下各关节运动速度 + double tcpAcc;//tcp加速度 + double joIntAcc[8];//关节运动下各关节加速度 + double joIntTemperature[6];//温度 + double joIntTorque[6];//输出扭矩 + double extJoIntTorques[6];//外部关节扭矩值 + double exTcpForceIntool[6];//当前工具坐标系下的外部末端力/力矩估计值 + quint8 dragState;//拖动使能状态 + quint8 sensor_connected_state;//力控传感器连接状态 + quint8 reserved; + quint32 matchingWord; +}; + +class EC8056 : public QObject +{ + Q_OBJECT + +public: + explicit EC8056(QObject* parent = nullptr); + void connectToHost(const QString& host, quint16 port= 8056); + +signals: + void dataReceived(const ECData& data); + void errorOccurred(const QString& error); + +private slots: + void onReadyRead(); + void onSocketError(QAbstractSocket::SocketError socketError); + +private: + QTcpSocket* socket; + QByteArray buffer; + + double readDouble(const QByteArray& buf, int& offset); + quint64 readUInt64(const QByteArray& buf, int& offset); + quint32 readUInt32(const QByteArray& buf, int& offset); + qint32 readInt32(const QByteArray& buf, int& offset); + quint8 readUInt8(const QByteArray& buf, int& offset); +}; + +class RobotController : public QObject +{ + Q_OBJECT +public: + explicit RobotController(QObject* parent = nullptr); + ~RobotController(); + + bool connectToRobot(const QString& ip, quint16 port = 8055); + void disconnectFromRobot(); + void sendCommand(const QString& cmd, const QJsonValue& params = QJsonArray(), int id = 1); + + void getRobotPose(); + void getRobotState();//停止状态 0，暂停状态 1，急停状态 2，运行状态 3，报警状态 4，碰撞状态 5 + void getJbiState();//0 停止状态,1 暂停状态,2 急停状态,3 运行状态,4 错误状态 + void getCurrentJobLine(); + void getRobotMode();//示教模式 0，自动模式 1，远程模式 2 + void runJbi(const QString& jbiFilename); + void pauseTask(); + void run(); + void continueTask(); + void setRobotPowerStatus(int status); + +signals: + void commandResponse(bool success, const QJsonObject& response); + +private slots: + void onReadyRead(); + void getPoint(); + +private: + QTcpSocket* socket; + + QTimer* timer; +}; + +class RobotArmControl : public QDialog +{ + Q_OBJECT + +public: + RobotArmControl(QWidget* parent = nullptr); + ~RobotArmControl(); + RobotController* robotController; + + +public Q_SLOTS: + void onCommandResponse(bool success, const QJsonObject& response); + void getTaskList(); + void getPose(); + + void connectRobotArm(); + void executeTask(); + void pauseTask(); + void continueTask(); + + void monitorRobotArm(const ECData& data); + +signals: + //void Opened(); + //void Closed(); + +private: + Ui::RobotArmControl_UI ui; + + EC8056* robotMonitor; + QStringListModel* m_pModel; +}; diff --git a/HPPA/RobotArmControl.ui b/HPPA/RobotArmControl.ui new file mode 100644 index 0000000..7b85f7d --- /dev/null +++ b/HPPA/RobotArmControl.ui @@ -0,0 +1,138 @@ + + + RobotArmControl_UI + + + + 0 + 0 + 424 + 364 + + + + Dialog + + + + + + 鏆傚仠浠诲姟 + + + + + + + 杩炴帴鏈烘鑷 + + + + + + + 鎵ц浠诲姟 + + + + + + + 鑾峰彇pose + + + + + + + 缁х画浠诲姟 + + + + + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + 0 + + + 0 + + + 0 + + + 0 + + + 0 + + + + + 鑾峰彇浠诲姟鍒楄〃 + + + + + + + + + + + + + QFrame::StyledPanel + + + QFrame::Raised + + + + + + z + + + Qt::AlignCenter + + + + + + + x + + + Qt::AlignCenter + + + + + + + y + + + Qt::AlignCenter + + + + + + + + + + +