QTcpSocket异步通讯控制机械臂
This commit is contained in:
tangchao0503
427
HPPA/RobotArmControl.cpp
Normal file
427
HPPA/RobotArmControl.cpp
Normal file
@ -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);
|
||||
|
||||
//<2F><><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>б<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͼ<EFBFBD><CDBC>
|
||||
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());
|
||||
}
|
||||
|
||||
////<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
|
||||
//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())
|
||||
{
|
||||
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʾ<EFBFBD>û<EFBFBD>ѡ<EFBFBD><D1A1><EFBFBD>ļ<EFBFBD>
|
||||
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;//ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>ṹ
|
||||
|
||||
QJsonObject params_set_servo_status;
|
||||
params_set_servo_status["status"] = 1;//ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>ṹ
|
||||
|
||||
//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<QAbstractSocket::SocketError>::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<quint8>(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());
|
||||
}
|
||||
Reference in New Issue
Block a user