640 lines
16 KiB
C++
640 lines
16 KiB
C++
#include "RobotArmControl.h"
|
||
|
||
RobotArmControl::RobotArmControl(QWidget* parent): QDialog(parent)
|
||
{
|
||
ui.setupUi(this);
|
||
robotController = new RobotController(this);
|
||
|
||
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(executeTaskWithoutHyperImager()));
|
||
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(QString str, const QJsonObject& response)
|
||
{
|
||
//qDebug() << "response:" << response;
|
||
|
||
QString re;
|
||
if (response.contains("result"))
|
||
{
|
||
re = response["result"].toVariant().toString();
|
||
ui.textEdit->append(str + " Result: " + re);
|
||
}
|
||
else if (response.contains("error"))
|
||
{
|
||
//auto delete11 = response["error"].toObject();
|
||
//qDebug() << "response[\"error\"]:" << delete11;
|
||
|
||
auto errorStr = response["error"].toObject()["message"].toString();
|
||
|
||
ui.textEdit->append(str + " Error: " + errorStr);
|
||
}
|
||
}
|
||
|
||
void RobotArmControl::getTaskList()
|
||
{
|
||
FileOperation* fileOperation = new FileOperation();
|
||
string directory = fileOperation->getDirectoryOfExe();
|
||
|
||
QString pythonScript = QString::fromStdString(directory) + "\\get_jbi_filename.py";
|
||
|
||
QProcess process;
|
||
|
||
process.start("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()
|
||
{
|
||
QJsonObject response;
|
||
bool x = robotController->getRobotPose(response);
|
||
onCommandResponse("getPose", response);
|
||
}
|
||
|
||
void RobotArmControl::connectRobotArm()
|
||
{
|
||
bool re = robotController->connectToRobot("192.168.1.100");
|
||
robotMonitor->connectToHost("192.168.1.100");
|
||
}
|
||
|
||
void RobotArmControl::executeTaskWithHyperImager()
|
||
{
|
||
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();
|
||
|
||
QJsonObject response;
|
||
bool x;
|
||
|
||
x = robotController->checkJbiExist(fileName, response);
|
||
onCommandResponse("checkJbiExist", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
|
||
x = robotController->setServoStatus(1, response);
|
||
onCommandResponse("setServoStatus", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
|
||
x = robotController->runJbi(fileName, response, true);
|
||
onCommandResponse("runJbi", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
|
||
void RobotArmControl::executeTaskWithoutHyperImager()
|
||
{
|
||
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();
|
||
|
||
QJsonObject response;
|
||
bool x;
|
||
|
||
x = robotController->checkJbiExist(fileName, response);
|
||
onCommandResponse("checkJbiExist", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
|
||
x = robotController->setServoStatus(1, response);
|
||
onCommandResponse("setServoStatus", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
|
||
x = robotController->runJbi(fileName, response, false);
|
||
onCommandResponse("runJbi", response);
|
||
if (!x)
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
|
||
void RobotArmControl::pauseTask()
|
||
{
|
||
QJsonObject response;
|
||
bool x;
|
||
|
||
x = robotController->pauseTask(response);
|
||
onCommandResponse("pauseTask", response);
|
||
}
|
||
|
||
void RobotArmControl::continueTask()
|
||
{
|
||
QJsonObject response;
|
||
bool x;
|
||
|
||
x = robotController->continueTask(response);
|
||
onCommandResponse("continueTask", response);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
RobotController::RobotController(QObject* parent) : QObject(parent), socket(new QTcpSocket(this))
|
||
{
|
||
//connect(socket, &QTcpSocket::readyRead, this, &RobotController::onReadyRead);
|
||
|
||
m_timer = new QTimer(this);
|
||
connect(m_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();
|
||
}
|
||
}
|
||
|
||
bool RobotController::processResponse(QJsonObject response, QString& result)
|
||
{
|
||
//qDebug() << "response:" << response;
|
||
|
||
if (response.contains("result"))
|
||
{
|
||
result = response["result"].toVariant().toString();
|
||
|
||
//qDebug() << "result1:" << result;
|
||
|
||
return true;
|
||
}
|
||
else if (response.contains("error"))
|
||
{
|
||
result = response["error"].toObject()["message"].toString();
|
||
//qDebug() << "result2:" << result;
|
||
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool RobotController::processResponse_getJbiState(QJsonObject response, QString& result)
|
||
{
|
||
//qDebug() << "response:" << response;
|
||
|
||
if (response.contains("result"))
|
||
{
|
||
QString resultStr = response["result"].toString();
|
||
|
||
QJsonDocument resultDoc = QJsonDocument::fromJson(resultStr.toUtf8());
|
||
QJsonObject resultObj = resultDoc.object();
|
||
//qDebug() << "resultObj:" << resultObj;
|
||
|
||
int runState = resultObj["runState"].toInt();
|
||
//qDebug() << "runState:" << runState;
|
||
|
||
result = QString::number(runState);
|
||
//qDebug() << "result:" << result;
|
||
|
||
return true;
|
||
}
|
||
else if (response.contains("error"))
|
||
{
|
||
result = response["error"].toObject()["message"].toString();
|
||
|
||
return false;
|
||
}
|
||
}
|
||
|
||
void RobotController::getPoint()
|
||
{
|
||
QJsonObject response;
|
||
getJbiState(response);//0 ֹͣ״̬,1 <20><>ͣ״̬,2 <20><>ͣ״̬,3 <20><><EFBFBD><EFBFBD>״̬,4 <20><><EFBFBD><EFBFBD>״̬
|
||
QString result;
|
||
bool x = processResponse_getJbiState(response, result);
|
||
//qDebug() << "getJbiState:" << result;
|
||
|
||
if (result.toInt() != 3)
|
||
{
|
||
m_timer->stop();
|
||
|
||
m_iCurrentJbiJobLine = 0;
|
||
m_iFileCounter = 0;
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3>ɼ<EFBFBD>
|
||
emit hsiRecordSignal(-1);
|
||
|
||
return;
|
||
}
|
||
|
||
QJsonObject response2;
|
||
getCurrentJobLine(response2);
|
||
QString result2;
|
||
bool x2 = processResponse(response2, result2);
|
||
|
||
int m_iCurrentJbiJobLine_tmp = result2.toInt();
|
||
|
||
|
||
if (m_iCurrentJbiJobLine_tmp != m_iCurrentJbiJobLine)
|
||
{
|
||
m_iFileCounter++;
|
||
|
||
qDebug() << "Changed! CurrentJobLine:" << m_iCurrentJbiJobLine_tmp;
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD><EFBFBD>ֹͣ<CDA3>ɼ<EFBFBD>
|
||
emit hsiRecordSignal(-1);
|
||
|
||
m_iCurrentJbiJobLine = m_iCurrentJbiJobLine_tmp;
|
||
|
||
//<2F><><EFBFBD><EFBFBD><EFBFBD>źţ<C5BA><C5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD>ɼ<EFBFBD>
|
||
emit hsiRecordSignal(m_iFileCounter);
|
||
}
|
||
}
|
||
|
||
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);
|
||
socket->waitForBytesWritten();
|
||
}
|
||
|
||
bool RobotController::onReadyRead(QJsonObject& re)
|
||
{
|
||
QByteArray data = socket->readAll();
|
||
QJsonDocument doc = QJsonDocument::fromJson(data);
|
||
if (!doc.isNull() && doc.isObject())
|
||
{
|
||
re = doc.object();
|
||
//qDebug() << "Received all:" << re;
|
||
|
||
if (re.contains("result"))
|
||
{
|
||
//qDebug() << "Received result:" << re["result"].toVariant();
|
||
return true;
|
||
}
|
||
else if (re.contains("error"))
|
||
{
|
||
//qDebug() << "Received error:" << re["error"];
|
||
return false;
|
||
}
|
||
|
||
//emit commandResponse(true, doc.object());
|
||
}
|
||
else
|
||
{
|
||
//emit commandResponse(false, QJsonObject());
|
||
|
||
re = QJsonObject();
|
||
return false;
|
||
}
|
||
}
|
||
|
||
bool RobotController::getRobotPose(QJsonObject& re)
|
||
{
|
||
sendCommand("getRobotPose");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::getRobotState(QJsonObject& re)
|
||
{
|
||
sendCommand("getRobotState");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::getJbiState(QJsonObject& re)
|
||
{
|
||
sendCommand("getJbiState");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::getCurrentJobLine(QJsonObject& re)
|
||
{
|
||
sendCommand("getCurrentJobLine");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::getRobotMode(QJsonObject& re)
|
||
{
|
||
sendCommand("getRobotMode");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::checkJbiExist(const QString& jbiFilename, QJsonObject& re)
|
||
{
|
||
QJsonObject paramsRunJbi;
|
||
paramsRunJbi["filename"] = jbiFilename;//ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>ṹ
|
||
|
||
sendCommand("checkJbiExist", paramsRunJbi);
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::setServoStatus(int status, QJsonObject& re)
|
||
{
|
||
QJsonObject params_set_servo_status;
|
||
params_set_servo_status["status"] = status;//ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>ṹ
|
||
|
||
sendCommand("set_servo_status", params_set_servo_status);
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::runJbi(const QString& jbiFilename, QJsonObject& re, bool isRecordHsi)
|
||
{
|
||
QJsonObject paramsRunJbi;
|
||
paramsRunJbi["filename"] = jbiFilename;//ʹ<>ö<EFBFBD><C3B6><EFBFBD><EFBFBD>ṹ
|
||
|
||
sendCommand("runJbi", paramsRunJbi);
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
if (isRecordHsi)
|
||
{
|
||
m_timer->start(1000);
|
||
}
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::pauseTask(QJsonObject& re)
|
||
{
|
||
sendCommand("pause");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::run(QJsonObject& re)
|
||
{
|
||
sendCommand("run");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::continueTask(QJsonObject& re)
|
||
{
|
||
sendCommand("run");
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
bool RobotController::setRobotPowerStatus(int status, QJsonObject& re)
|
||
{
|
||
sendCommand("set_robot_power_status", QJsonArray{ status });
|
||
socket->waitForReadyRead(m_iTimeout);
|
||
|
||
return onReadyRead(re);
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
|
||
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());
|
||
}
|