diff --git a/plugin/controller/src/main/java/DeepBlueSim.java b/plugin/controller/src/main/java/DeepBlueSim.java
index 8288429f..0dc8e010 100644
--- a/plugin/controller/src/main/java/DeepBlueSim.java
+++ b/plugin/controller/src/main/java/DeepBlueSim.java
@@ -91,20 +91,7 @@ public void uncaughtException(Thread arg0, Throwable arg1) {
             webotsSupervisorSim.set("self.position.z", pos[2]);
         });
 
-        // Pause the simulator if it hasn't taken any steps in the last 1-2 seconds
-        // so it doesn't suck up CPU.
         Timer simPauseTimer = new Timer();
-        simPauseTimer.schedule(new TimerTask() {
-            @Override
-            public void run() {
-                if (System.currentTimeMillis() - lastStepMillis > 1000) {
-                    queuedMessages.add(() -> {
-                        updateUsersSimulationSpeed(robot);
-                        robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE);
-                    });
-                }
-            }
-        }, 1000, 1000);
 
         // Whenever the robot time changes, step the simulation until just past that time
         robotTimeSecCallbackStore = timeSynchronizerSim.registerValueChangedCallback("robotTimeSec", new StringCallback() {
@@ -135,6 +122,23 @@ public synchronized void callback(String name, String value) {
                     // Unpause if necessary
                     robot.simulationSetMode(usersSimulationSpeed);
                     boolean isDone = (robot.step(basicTimeStep) == -1);
+
+                    // If that was our first step, schedule a task to pause the simulator if it
+                    // doesn't taken any steps for 1-2 seconds so it doesn't suck up CPU.
+                    if (lastStepMillis == 0) {
+                        simPauseTimer.schedule(new TimerTask() {
+                            @Override
+                            public void run() {
+                                if (System.currentTimeMillis() - lastStepMillis > 1000) {
+                                    queuedMessages.add(() -> {
+                                        updateUsersSimulationSpeed(robot);
+                                        robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE);
+                                    });
+                                }
+                            }
+                        }, 1000, 1000);
+                    }
+
                     lastStepMillis = System.currentTimeMillis();
                     timeSynchronizerSim.set("simTimeSec", robot.getTime());
                     if (isDone) {
@@ -161,6 +165,9 @@ public synchronized void callback(String name, String value) {
         }
         SimRegisterer.connectDevices();
 
+        // Pause the simulation until either the robot code tells us to proceed or the
+        // user does.
+        robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE);
 
         // Connect to the robot code
         try {
@@ -182,7 +189,27 @@ public synchronized void callback(String name, String value) {
         // Process incoming messages until simulation finishes
         try {
             while (isDoneFuture.getNow(false).booleanValue() == false) {
-                queuedMessages.takeFirst().run();
+                if (timeSynchronizerSim.get("robotTimeSec") != null || !queuedMessages.isEmpty()) {
+                    // Either there is a message waiting or it is ok to wait for it because the
+                    // robot code will tell us when to step the simulation.
+                    queuedMessages.takeFirst().run();
+                } else if (timeSynchronizerSim.get("robotTimeSec") == null
+                        && robot.simulationGetMode() != Supervisor.SIMULATION_MODE_PAUSE) {
+                    // The robot code isn't going to tell us when to step the simulation and the
+                    // user has unpaused it.
+                    Simulation.runPeriodicMethods();
+                    if (robot.step(basicTimeStep) == -1) {
+                        break;
+                    }
+                } else {
+                    // The simulation is paused and robot code isn't in control so wait a beat
+                    // before checking again (so we don't suck up all the CPU)
+                    Thread.sleep(20);
+                    // Process any pending user interface events.
+                    if (robot.step(0) == -1) {
+                        break;
+                    }
+                }
             }
         } catch (Exception ex) {
             throw new RuntimeException("Exception while waiting for simulation to be done", ex);