fixed controller deletion

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();
     }
 }