添加管控软件和相机的通讯
This commit is contained in:
@@ -4,6 +4,7 @@
|
||||
#include "../modbus/modbustcpmanager.h"
|
||||
|
||||
#include <QTimer>
|
||||
#include <QDebug>
|
||||
|
||||
namespace {
|
||||
|
||||
@@ -91,6 +92,18 @@ void DeviceService::startPolling()
|
||||
pollStatus();
|
||||
}
|
||||
|
||||
DeviceService::~DeviceService()
|
||||
{
|
||||
m_pollTimer->stop();
|
||||
m_terminationActive = true;
|
||||
m_terminationCallback = OperationCallback();
|
||||
++m_visionGeneration;
|
||||
// Modbus abort invokes pending callbacks; keep our state alive until they drain.
|
||||
m_modbus->abortAll("Device service is shutting down");
|
||||
delete m_modbus;
|
||||
m_modbus = nullptr;
|
||||
}
|
||||
|
||||
void DeviceService::stopPolling()
|
||||
{
|
||||
m_pollTimer->stop();
|
||||
@@ -952,3 +965,160 @@ bool DeviceService::productionActionBusy() const
|
||||
{
|
||||
return m_productionActionBusy || m_modbus->isBusy();
|
||||
}
|
||||
|
||||
bool DeviceService::checkVisionAccess(QString *error) const
|
||||
{
|
||||
if (m_terminationActive || ConfigManager::instance().simulationMode()
|
||||
|| !ConfigManager::instance().productionEnabled()) {
|
||||
if (error) *error = "Vision PLC access is disabled during termination or simulation";
|
||||
return false;
|
||||
}
|
||||
if (m_modbus->config().serverAddress == 0) {
|
||||
if (error) *error = "Vision PLC writes require an addressed response, not broadcast";
|
||||
return false;
|
||||
}
|
||||
return ConfigManager::instance().visionConfig().isValid(error);
|
||||
}
|
||||
|
||||
void DeviceService::readRobotPhotoPose(const PoseCallback &callback)
|
||||
{
|
||||
QString error;
|
||||
if (!checkVisionAccess(&error)) {
|
||||
if (callback) callback(false, RobotPose(), error);
|
||||
return;
|
||||
}
|
||||
const VisionConfig config = ConfigManager::instance().visionConfig();
|
||||
const quint64 generation = m_visionGeneration;
|
||||
if (!m_modbus->readRReals(config.photoPoseR, 6,
|
||||
[this, callback, generation](bool success, const QVector<double> &values, const QString &text) {
|
||||
RobotPose pose;
|
||||
pose.xyzabc = values;
|
||||
if (callback) {
|
||||
callback(success && generation == m_visionGeneration && pose.isValid(), pose,
|
||||
generation == m_visionGeneration ? text : "Vision pose read canceled");
|
||||
}
|
||||
}, config.lowWordFirst)) {
|
||||
if (callback) callback(false, RobotPose(), "Invalid R40 read request");
|
||||
}
|
||||
}
|
||||
|
||||
void DeviceService::writeRobotGraspPose(const RobotPose &pose,
|
||||
const VisionWriteCallback &callback,
|
||||
const std::function<bool()> &beforeSend)
|
||||
{
|
||||
QString error;
|
||||
if (!checkVisionAccess(&error) || !pose.isValid()) {
|
||||
if (callback) callback(VisionWriteOutcome::NotSent,
|
||||
error.isEmpty() ? "Invalid robot grasp pose" : error);
|
||||
return;
|
||||
}
|
||||
const VisionConfig config = ConfigManager::instance().visionConfig();
|
||||
if (!m_modbus->writeRReals(config.graspPoseR, pose.xyzabc, callback, config.lowWordFirst, beforeSend)) {
|
||||
if (callback) callback(VisionWriteOutcome::NotSent, "Invalid or overflowing R60 REAL request");
|
||||
return;
|
||||
}
|
||||
for (int axis = 0; axis < pose.xyzabc.size(); ++axis) {
|
||||
const double rounded = double(float(pose.xyzabc.at(axis)));
|
||||
if (rounded != pose.xyzabc.at(axis)) {
|
||||
qInfo().nospace() << "Vision R60 axis " << axis << " REAL rounding: "
|
||||
<< QString::number(pose.xyzabc.at(axis), 'g', 17) << " -> "
|
||||
<< QString::number(rounded, 'g', 17);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void DeviceService::readVisionHandshake(const HandshakeCallback &callback)
|
||||
{
|
||||
QString error;
|
||||
if (!checkVisionAccess(&error)) {
|
||||
if (callback) callback(false, VisionHandshake(), error);
|
||||
return;
|
||||
}
|
||||
const VisionConfig config = ConfigManager::instance().visionConfig();
|
||||
if (config.photoRequestCoil == config.robotReceivedCoil + 1
|
||||
&& config.comparingCoil == config.robotReceivedCoil + 2
|
||||
&& config.comparisonErrorCoil == config.robotReceivedCoil + 3
|
||||
&& config.comparisonPassedCoil == config.robotReceivedCoil + 4) {
|
||||
const quint64 generation = m_visionGeneration;
|
||||
m_modbus->readCoils(config.robotReceivedCoil, 5,
|
||||
[this, callback, generation](bool success, const QVector<bool> &values, const QString &text) {
|
||||
VisionHandshake state;
|
||||
if (success && values.size() == 5 && generation == m_visionGeneration) {
|
||||
state.robotReceived = values.at(0);
|
||||
state.photoRequest = values.at(1);
|
||||
state.comparing = values.at(2);
|
||||
state.comparisonError = values.at(3);
|
||||
state.comparisonPassed = values.at(4);
|
||||
if (callback) callback(true, state, QString());
|
||||
} else if (callback) {
|
||||
callback(false, state, text.isEmpty() ? "Vision handshake read canceled or incomplete" : text);
|
||||
}
|
||||
}, true);
|
||||
return;
|
||||
}
|
||||
readVisionHandshakeStep(0, VisionHandshake(), m_visionGeneration, callback);
|
||||
}
|
||||
|
||||
void DeviceService::readVisionHandshakeStep(int index, const VisionHandshake &state,
|
||||
quint64 generation, const HandshakeCallback &callback)
|
||||
{
|
||||
if (generation != m_visionGeneration) {
|
||||
if (callback) callback(false, VisionHandshake(), "Vision handshake read canceled");
|
||||
return;
|
||||
}
|
||||
if (index == 5) {
|
||||
if (callback) callback(true, state, QString());
|
||||
return;
|
||||
}
|
||||
const VisionConfig config = ConfigManager::instance().visionConfig();
|
||||
const int addresses[] = {config.robotReceivedCoil, config.photoRequestCoil,
|
||||
config.comparingCoil, config.comparisonErrorCoil,
|
||||
config.comparisonPassedCoil};
|
||||
m_modbus->readVisionCoil(addresses[index],
|
||||
[this, index, state, generation, callback](bool success, bool value, const QString &error) {
|
||||
if (!success) {
|
||||
if (callback) callback(false, VisionHandshake(), error);
|
||||
return;
|
||||
}
|
||||
VisionHandshake next = state;
|
||||
switch (index) {
|
||||
case 0: next.robotReceived = value; break;
|
||||
case 1: next.photoRequest = value; break;
|
||||
case 2: next.comparing = value; break;
|
||||
case 3: next.comparisonError = value; break;
|
||||
case 4: next.comparisonPassed = value; break;
|
||||
}
|
||||
readVisionHandshakeStep(index + 1, next, generation, callback);
|
||||
});
|
||||
}
|
||||
|
||||
void DeviceService::writeVisionSignal(VisionSignal signal, bool value,
|
||||
const VisionWriteCallback &callback,
|
||||
const std::function<bool()> &beforeSend)
|
||||
{
|
||||
QString error;
|
||||
if (!checkVisionAccess(&error)) {
|
||||
if (callback) callback(VisionWriteOutcome::NotSent, error);
|
||||
return;
|
||||
}
|
||||
const VisionConfig config = ConfigManager::instance().visionConfig();
|
||||
int address = -1;
|
||||
QString name;
|
||||
switch (signal) {
|
||||
case Comparing: address = config.comparingCoil; name = "comparing"; break;
|
||||
case ComparisonError: address = config.comparisonErrorCoil; name = "comparisonError"; break;
|
||||
case ComparisonPassed: address = config.comparisonPassedCoil; name = "comparisonPassed"; break;
|
||||
default: break;
|
||||
}
|
||||
if (address < 0 || !config.writableSignals.contains(name)) {
|
||||
if (callback) callback(VisionWriteOutcome::NotSent, "Vision coil is not owned by ssp");
|
||||
return;
|
||||
}
|
||||
m_modbus->writeVisionCoil(address, value, callback, beforeSend);
|
||||
}
|
||||
|
||||
void DeviceService::cancelVisionRequests(const QString &reason)
|
||||
{
|
||||
++m_visionGeneration;
|
||||
m_modbus->cancelVisionRequests(reason);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user