640 lines
17 KiB
C++
640 lines
17 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());
|
|
}
|
|
|
|
////先清除数据，然后添加数据
|
|
//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())
|
|
{
|
|
//弹窗提示用户选择文件
|
|
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())
|
|
{
|
|
//弹窗提示用户选择文件
|
|
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 暂停状态,2 急停状态,3 运行状态,4 错误状态
|
|
QString result;
|
|
bool x = processResponse_getJbiState(response, result);
|
|
//qDebug() << "getJbiState:" << result;
|
|
|
|
if (result.toInt() != 3)
|
|
{
|
|
m_timer->stop();
|
|
|
|
m_iCurrentJbiJobLine = 0;
|
|
m_iFileCounter = 0;
|
|
|
|
//发射信号，相机停止采集
|
|
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;
|
|
//发射信号，相机停止采集
|
|
emit hsiRecordSignal(-1);
|
|
|
|
m_iCurrentJbiJobLine = m_iCurrentJbiJobLine_tmp;
|
|
|
|
//发射信号，相机开始采集
|
|
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;//使用对象结构
|
|
|
|
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;//使用对象结构
|
|
|
|
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;//使用对象结构
|
|
|
|
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());
|
|
}
|