commandToolUpdate

modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp

@@ -15,6 +15,7 @@ int main(int argc, char ** argv)
     networking::Networking net;
     std::atomic<bool> monitorSTM{false};
     std::atomic<bool> monitorUSS{false};
+    std::atomic<bool> speed{0.0f};
     std::atomic<bool> running{true};
     std::mutex outputMutex;
     auto conn = Connection::create(net);
@@ -40,8 +41,10 @@ int main(int argc, char ** argv)
             while (running)
             {
                 int distance = uss.getDistance();
-                //if (distance > 20)
-                    //conn->setSpeed(0);
+                if (distance <= 20)
+                    conn->setSpeed(0);
+                else
+                    conn->setSpeed(speed);
 
                 if (!monitorUSS)
                     continue;
@@ -96,7 +99,7 @@ int main(int argc, char ** argv)
                 value = value >= 0 ? MAX_SPEED : -MAX_SPEED;
                 std::cout << "WARNING: Speed set to 1.0 due to risk of collision!\n";
             }
-            conn->setSpeed(value);
+            speed = value;
         }
         else std::cout << "Error: Unknown command!\n";
     }