feat:初始化wuhan_QT界面代码
This commit is contained in:
xu
@@ -0,0 +1,223 @@
|
||||
#include "mockvehicle.h"
|
||||
#include <QSettings>
|
||||
#include <QJsonDocument>
|
||||
#include <QJsonObject>
|
||||
#include <QJsonArray>
|
||||
#include <QHostAddress>
|
||||
#include <QNetworkDatagram>
|
||||
#include <QtMath>
|
||||
|
||||
MockVehicle::MockVehicle(QObject* parent)
|
||||
: QObject(parent)
|
||||
{
|
||||
QSettings s;
|
||||
m_localPort = s.value("net/local_port", 9000).toInt(); // 发送到遥控端
|
||||
m_remotePort = s.value("net/remote_port", 9001).toInt(); // 本端监听
|
||||
m_enabled = s.value("net/mock", true).toBool();
|
||||
m_plantSelfCheckFault = s.value("net/selfcheck_fault", false).toBool();
|
||||
|
||||
if (!m_enabled)
|
||||
return;
|
||||
|
||||
m_socket = new QUdpSocket(this);
|
||||
m_socket->bind(QHostAddress::AnyIPv4, m_remotePort,
|
||||
QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
|
||||
connect(m_socket, &QUdpSocket::readyRead, this, &MockVehicle::onReadyRead);
|
||||
|
||||
m_sendTimer = new QTimer(this);
|
||||
connect(m_sendTimer, &QTimer::timeout, this, &MockVehicle::onSendTick);
|
||||
m_sendTimer->start(500);
|
||||
|
||||
m_selfCheckTimer = new QTimer(this);
|
||||
connect(m_selfCheckTimer, &QTimer::timeout, this, &MockVehicle::onSelfCheckTick);
|
||||
}
|
||||
|
||||
void MockVehicle::sendJson(const QJsonObject& obj)
|
||||
{
|
||||
QByteArray data = QJsonDocument(obj).toJson(QJsonDocument::Compact);
|
||||
m_socket->writeDatagram(data, QHostAddress("127.0.0.1"), m_localPort);
|
||||
}
|
||||
|
||||
void MockVehicle::onReadyRead()
|
||||
{
|
||||
while (m_socket->hasPendingDatagrams()) {
|
||||
QNetworkDatagram dg = m_socket->receiveDatagram();
|
||||
QJsonParseError err;
|
||||
QJsonDocument doc = QJsonDocument::fromJson(dg.data(), &err);
|
||||
if (err.error == QJsonParseError::NoError && doc.isObject())
|
||||
handleCommand(doc.object());
|
||||
}
|
||||
}
|
||||
|
||||
void MockVehicle::handleCommand(const QJsonObject& o)
|
||||
{
|
||||
const QString t = o.value("t").toString();
|
||||
|
||||
if (t == "chassis_move") {
|
||||
const QString dir = o.value("dir").toString();
|
||||
m_speed = o.value("speed").toDouble();
|
||||
if (dir == "stop") m_speed = 0;
|
||||
} else if (t == "arm_move") {
|
||||
int d = o.value("dir").toInt();
|
||||
if (d == 0) return;
|
||||
if (m_js.size() >= 6)
|
||||
m_js[1] = qBound(-90.0, m_js[1] + d * 1.0, 90.0);
|
||||
} else if (t == "arm_return") {
|
||||
m_armReturn = "returning";
|
||||
m_armHomed = false;
|
||||
} else if (t == "arm_pause") {
|
||||
m_armReturn = "idle";
|
||||
} else if (t == "gas_start") {
|
||||
m_gasRunning = true;
|
||||
} else if (t == "gas_stop") {
|
||||
m_gasRunning = false;
|
||||
} else if (t == "wash_start") {
|
||||
m_washRunning = true;
|
||||
m_washMode = (o.value("mode").toString() == "disinfect") ? "手动消毒" : "手动消除";
|
||||
m_pPlunger = true; m_pAir = true; m_pLeaf = true;
|
||||
} else if (t == "wash_stop") {
|
||||
m_washRunning = false;
|
||||
m_pPlunger = false; m_pAir = false; m_pLeaf = false;
|
||||
} else if (t == "ball_valve") {
|
||||
int v = o.value("valve").toInt();
|
||||
bool open = o.value("open").toBool();
|
||||
if (v == 1) m_valve1Open = open;
|
||||
else m_valve2Open = open;
|
||||
} else if (t == "pump_power") {
|
||||
const QString p = o.value("pump").toString();
|
||||
bool on = o.value("on").toBool();
|
||||
if (p == "plunger") m_pPlunger = on;
|
||||
else if (p == "air") m_pAir = on;
|
||||
else if (p == "leaf") m_pLeaf = on;
|
||||
} else if (t == "pump_rpm") {
|
||||
const QString p = o.value("pump").toString();
|
||||
int rpm = o.value("rpm").toInt();
|
||||
if (p == "plunger") m_plungerRpm = rpm;
|
||||
else if (p == "air") m_airRpm = rpm;
|
||||
else if (p == "leaf") m_leafRpm = rpm;
|
||||
} else if (t == "selfcheck_start") {
|
||||
m_selfCheckActive = true;
|
||||
m_selfCheckIndex = 0;
|
||||
m_selfCheckTimer->start(450);
|
||||
} else if (t == "estop") {
|
||||
m_estop = true;
|
||||
m_washRunning = false; m_gasRunning = false;
|
||||
m_pPlunger = false; m_pAir = false; m_pLeaf = false;
|
||||
QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = true;
|
||||
sendJson(o2);
|
||||
} else if (t == "estop_reset") {
|
||||
m_estop = false;
|
||||
QJsonObject o2; o2["t"] = "estop"; o2["triggered"] = false;
|
||||
sendJson(o2);
|
||||
}
|
||||
// set_param 仅用于演示，无需额外处理
|
||||
}
|
||||
|
||||
void MockVehicle::onSendTick()
|
||||
{
|
||||
sendHeartbeat();
|
||||
sendTelemetry();
|
||||
}
|
||||
|
||||
void MockVehicle::sendHeartbeat()
|
||||
{
|
||||
QJsonObject o; o["t"] = "heartbeat"; o["ok"] = true;
|
||||
sendJson(o);
|
||||
}
|
||||
|
||||
void MockVehicle::sendTelemetry()
|
||||
{
|
||||
// 底盘
|
||||
{
|
||||
QJsonObject o;
|
||||
QJsonArray faults;
|
||||
if (m_estop) faults << QStringLiteral("急停触发");
|
||||
o["t"] = "chassis";
|
||||
o["soc"] = 85; o["volt"] = 360.0; o["speed"] = m_speed;
|
||||
o["handbrake"] = true; o["faults"] = faults;
|
||||
sendJson(o);
|
||||
}
|
||||
// 机械臂
|
||||
{
|
||||
if (m_armReturn == "returning") {
|
||||
bool allZero = true;
|
||||
for (double& a : m_js) { if (qAbs(a) > 1.0) a *= 0.7; else a = 0; if (qAbs(a) > 0.1) allZero = false; }
|
||||
if (allZero) { m_armReturn = "done"; m_armHomed = true; }
|
||||
}
|
||||
QJsonObject o;
|
||||
QJsonArray js;
|
||||
for (double a : m_js) js << a;
|
||||
o["t"] = "arm";
|
||||
o["js"] = js;
|
||||
o["pos"] = QString("X:%1 Y:%2 Z:%3").arg(1200).arg(300).arg(800);
|
||||
o["pose"] = "RX:0 RY:0 RZ:0";
|
||||
o["vel"] = (m_armReturn == "returning") ? 0.5 : 0.0;
|
||||
o["return_state"] = m_armReturn;
|
||||
o["homed"] = m_armHomed;
|
||||
sendJson(o);
|
||||
}
|
||||
// 洗消
|
||||
{
|
||||
QJsonObject o;
|
||||
o["t"] = "wash";
|
||||
o["mode"] = m_washRunning ? m_washMode : "--";
|
||||
o["flowA"] = m_washRunning ? 20.0 : 0.0;
|
||||
o["flowB"] = m_washRunning ? 0.5 : 0.0;
|
||||
o["gasFlow"] = m_washRunning ? 100.0 : 0.0;
|
||||
o["press"] = m_washRunning ? 0.3 : 0.0;
|
||||
o["airPress"] = m_washRunning ? 0.35 : 0.0;
|
||||
o["foam"] = (m_washMode == "手动消毒") ? 6.0 : 8.0;
|
||||
o["faults"] = QJsonArray();
|
||||
sendJson(o);
|
||||
}
|
||||
// 液位
|
||||
{
|
||||
QJsonObject o; o["t"] = "tank"; o["a"] = 100.0; o["foam"] = 30.0;
|
||||
sendJson(o);
|
||||
}
|
||||
// 气体检测仪
|
||||
{
|
||||
static int step = 0; step++;
|
||||
double conc = m_gasRunning ? 250.0 + 50.0 * qSin(step * 0.4) : 0.0;
|
||||
QJsonObject o;
|
||||
o["t"] = "gas";
|
||||
o["host"] = m_gasRunning ? "detecting" : "standby";
|
||||
o["type"] = m_gasRunning ? "S类" : "--";
|
||||
o["level"] = m_gasRunning ? "2级" : "--";
|
||||
o["conc"] = qMax(0.0, conc);
|
||||
sendJson(o);
|
||||
}
|
||||
// 球阀
|
||||
{
|
||||
QJsonObject o;
|
||||
o["t"] = "valve";
|
||||
o["d1open"] = m_valve1Open; o["d1close"] = !m_valve1Open;
|
||||
o["d2open"] = m_valve2Open; o["d2close"] = !m_valve2Open;
|
||||
o["fault"] = "";
|
||||
sendJson(o);
|
||||
}
|
||||
// 泵
|
||||
{
|
||||
QJsonObject o;
|
||||
o["t"] = "pump";
|
||||
o["plunger"] = m_pPlunger; o["air"] = m_pAir; o["leaf"] = m_pLeaf;
|
||||
o["faults"] = QJsonArray();
|
||||
sendJson(o);
|
||||
}
|
||||
}
|
||||
|
||||
void MockVehicle::onSelfCheckTick()
|
||||
{
|
||||
// 自检 10 项：通信系统/底盘通信/机械臂通信/洗消通信/传感器信号/清水液位/球阀D1开/球阀D1关/球阀D2开/球阀D2关
|
||||
bool ok = true;
|
||||
if (m_plantSelfCheckFault && m_selfCheckIndex == 6) ok = false; // 球阀D1关闭故障
|
||||
QJsonObject o; o["t"] = "selfcheck"; o["item"] = m_selfCheckIndex; o["ok"] = ok;
|
||||
sendJson(o);
|
||||
m_selfCheckIndex++;
|
||||
if (m_selfCheckIndex >= 10) {
|
||||
m_selfCheckTimer->stop();
|
||||
m_selfCheckActive = false;
|
||||
QJsonObject o2; o2["t"] = "selfcheck_done"; o2["all_ok"] = !m_plantSelfCheckFault;
|
||||
sendJson(o2);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user