Stop the robots when the stop receiving packages for 1 sec

RoboCup-SSL · Jan 3, 2024 · eeccdb6 · eeccdb6
1 parent 3db783a
commit eeccdb6
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 1 deletion.
diff --git 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
@@ -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
@@ -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();
 }