From 1676f386be0d022eda440026ff3207ff34b7ded2 Mon Sep 17 00:00:00 2001 From: Bollos00 Date: Sat, 2 Oct 2021 01:15:15 -0300 Subject: [PATCH 1/2] Stop the robots when the stop receiving packages for 1 sec --- include/sslworld.h | 4 ++++ src/mainwindow.cpp | 38 +++++++++++++++++++++++++++++++++++++- src/sslworld.cpp | 7 +++++++ 3 files changed, 48 insertions(+), 1 deletion(-) 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..56a4ac4c 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,42 @@ 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(); +// glwidget->ssl->robots[index]->kicker->setRoller(0); +// glwidget->ssl->robots[index]->kicker->kick(0, 0); + } + } + // 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(); +// glwidget->ssl->robots[index]->kicker->setRoller(0); +// glwidget->ssl->robots[index]->kicker->kick(0, 0); + } + } + } } 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(); } From 3ab63581dcab0dc5508e0282b2db0bb9a91d5d1a Mon Sep 17 00:00:00 2001 From: Bollos00 Date: Tue, 15 Feb 2022 11:43:08 -0300 Subject: [PATCH 2/2] review fix --- src/mainwindow.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/mainwindow.cpp b/src/mainwindow.cpp index 56a4ac4c..0460c905 100644 --- a/src/mainwindow.cpp +++ b/src/mainwindow.cpp @@ -420,13 +420,10 @@ void MainWindow::update() { const int index = glwidget->ssl->robotIndex(i, BLUE-1); - if(index == -1 || - glwidget->ssl->robots[index] == nullptr) + if(index == -1 || glwidget->ssl->robots[index] == nullptr) continue; glwidget->ssl->robots[index]->resetSpeeds(); -// glwidget->ssl->robots[index]->kicker->setRoller(0); -// glwidget->ssl->robots[index]->kicker->kick(0, 0); } } // Stops yellow robots from moving if no package has been received for 1 second @@ -436,13 +433,10 @@ void MainWindow::update() { const int index = glwidget->ssl->robotIndex(i, YELLOW-1); - if(index == -1 || - glwidget->ssl->robots[index] == nullptr) + if(index == -1 || glwidget->ssl->robots[index] == nullptr) continue; glwidget->ssl->robots[index]->resetSpeeds(); -// glwidget->ssl->robots[index]->kicker->setRoller(0); -// glwidget->ssl->robots[index]->kicker->kick(0, 0); } } }