diff --git a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
index 82caa574643aa2302f0f88a9278f38343e14d28f..c1a2369f9c01964bf8282af46edbb34c97ba26d6 100644
--- a/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
+++ b/source/RobotAPI/libraries/RobotStatechartHelpers/VelocityControllerHelper.cpp
@@ -79,13 +79,14 @@ void VelocityControllerHelper::setNullSpaceControl(bool enabled)
 
 void VelocityControllerHelper::cleanup()
 {
-    controller->deactivateController();
     if (controllerCreated)
     {
-        while (controller->isControllerActive())
-        {
-            TimeUtil::SleepMS(1);
-        }
-        controller->deleteController();
+        // delete controller only if it was created
+        controller->deactivateAndDeleteController();
+    }
+    else
+    {
+        // if the controller existed, only deactivate it
+        controller->deactivateController();
     }
 }