diff --git a/include/sslworld.h b/include/sslworld.h index ece6c55d..60915835 100644 --- a/include/sslworld.h +++ b/include/sslworld.h @@ -111,6 +111,10 @@ class SSLWorld : public QObject QUdpSocket *simControlSocket; QUdpSocket *blueControlSocket; QUdpSocket *yellowControlSocket; + + QElapsedTimer elapsedLastPackageBlue; + QElapsedTimer elapsedLastPackageYellow; + bool updatedCursor; Robot* robots[MAX_ROBOT_COUNT*2]{}; int sendGeomCount; diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 35c75a49..0460c905 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -93,7 +93,7 @@ MainWindow::MainWindow(QWidget *parent) QObject::connect(simControlSocket,SIGNAL(readyRead()),this,SLOT(simControlSocketReady())); QObject::connect(blueControlSocket,SIGNAL(readyRead()),this,SLOT(blueControlSocketReady())); QObject::connect(yellowControlSocket,SIGNAL(readyRead()),this,SLOT(yellowControlSocketReady())); - + glwidget->ssl->visionServer = visionServer; glwidget->ssl->commandSocket = commandSocket; glwidget->ssl->blueStatusSocket = blueStatusSocket; @@ -410,6 +410,36 @@ void MainWindow::update() noiselabel->setVisible(configwidget->noise()); cursorlabel->setText(QString("Cursor: [X=%1;Y=%2;Z=%3]").arg(dRealToStr(glwidget->ssl->cursor_x)).arg(dRealToStr(glwidget->ssl->cursor_y)).arg(dRealToStr(glwidget->ssl->cursor_z))); statusWidget->update(); + + if(glwidget != nullptr && glwidget->ssl != nullptr) + { + // Stops blue robots from moving if no package has been received for 1 second + if(glwidget->ssl->elapsedLastPackageBlue.nsecsElapsed()*1e-9 > 1) + { + for(int i=0; i < glwidget->cfg->Robots_Count(); ++i) + { + const int index = glwidget->ssl->robotIndex(i, BLUE-1); + + if(index == -1 || glwidget->ssl->robots[index] == nullptr) + continue; + + glwidget->ssl->robots[index]->resetSpeeds(); + } + } + // Stops yellow robots from moving if no package has been received for 1 second + if(glwidget->ssl->elapsedLastPackageYellow.nsecsElapsed()*1e-9 > 1) + { + for(int i=0; i < glwidget->cfg->Robots_Count(); ++i) + { + const int index = glwidget->ssl->robotIndex(i, YELLOW-1); + + if(index == -1 || glwidget->ssl->robots[index] == nullptr) + continue; + + glwidget->ssl->robots[index]->resetSpeeds(); + } + } + } } void MainWindow::updateRobotLabel() diff --git a/src/sslworld.cpp b/src/sslworld.cpp index 7161669d..5cc33755 100644 --- a/src/sslworld.cpp +++ b/src/sslworld.cpp @@ -313,6 +313,9 @@ SSLWorld::SSLWorld(QGLWidget* parent, ConfigWidget* _cfg, RobotsFormation *form1 } restartRequired = false; + + elapsedLastPackageBlue.start(); + elapsedLastPackageYellow.start(); } int SSLWorld::robotIndex(int robot,int team) { @@ -705,6 +708,8 @@ void SSLWorld::blueControlSocketReady() { robotControlResponse.SerializeToArray(buffer.data(), buffer.size()); blueControlSocket->writeDatagram(buffer.data(), buffer.size(), datagram.senderAddress(), datagram.senderPort()); } + + elapsedLastPackageBlue.start(); } void SSLWorld::yellowControlSocketReady() { @@ -723,6 +728,8 @@ void SSLWorld::yellowControlSocketReady() { robotControlResponse.SerializeToArray(buffer.data(), buffer.size()); yellowControlSocket->writeDatagram(buffer.data(), buffer.size(), datagram.senderAddress(), datagram.senderPort()); } + + elapsedLastPackageYellow.start(); }