diff --git a/selflocalization/selflocalize.py b/selflocalization/selflocalize.py
index 0721080..7780709 100644
--- a/selflocalization/selflocalize.py
+++ b/selflocalization/selflocalize.py
@@ -249,7 +249,9 @@ try:
 
         particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
         est_pose, certainty = particle.estimate_pose(particles) # The estimate of the robots current pose
-        print(certainty)
+        print("Certainty: ", certainty)
+        if certainty < 1:
+            break
 
         if showGUI:
             # Draw map
@@ -261,7 +263,7 @@ try:
             # Show world
             cv2.imshow(WIN_World, world)
 
-    # drive_towards_middle(est_pose, arlo)
+    drive_towards_middle(est_pose, arlo)
 
 
 finally: