-

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

@@ -60,6 +60,7 @@ int main(int argc, char ** argv)
     std::atomic<bool> monitorSTM{false};
     std::atomic<bool> monitorUSS{false};
     std::atomic<float> speed{0.0f};
+    std::atomic<float> stopDistance{0};
     std::mutex outputMutex;
     UltrasonicSensor sensor{readDevId()};
     car::StreamMeanFilter streamMeanFilter{5};
@@ -85,7 +86,7 @@ int main(int argc, char ** argv)
         {
             auto originalDistance = sensor.getDistance();
             auto filteredDistance = streamMeanFilter.moveWindow(originalDistance);
-            if (filteredDistance <= 50)
+            if (filteredDistance <= stopDistance)
                 conn->setSpeed(BRAKE_SPEED);
             else
                 conn->setSpeed(speed);
@@ -138,6 +139,7 @@ int main(int argc, char ** argv)
                         std::cout << "WARNING: Speed set to 1.0 due to risk of collision!\n";
                     }
                     speed = value;
+                    stopDistance = speed.load() * 100 * 15;
                     std::cout << "Setting speed to: " << value << "\n";
                 }
             }