Added support for simulation time.

This allows control loops to behave more predictably in the face of the
simulator running at non-realtime speeds.

Change-Id: I3508ed7ad316a3bf8b2c54b68c93baaf8cc4d941
Closes: artf2607

Conflicts:
	wpilibc/wpilibC++Sim/include/Timer.h
	wpilibc/wpilibC++Sim/src/Utility.cpp

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/DriverStation.java

@@ -96,8 +96,10 @@ public class DriverStation implements IInputOutput {
         MainNode.subscribe("ds/state", GzDriverStation.DriverStation.getDefaultInstance(),
     		new SubscriberCallback<GzDriverStation.DriverStation>() {
 				@Override public void callback(GzDriverStation.DriverStation msg) {
+					state = msg;
+					m_newControlData = true;
 					synchronized (m_dataSem) {
-						state = msg;
+						m_dataSem.notifyAll();
 					}
 				}
 			}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java

@@ -144,6 +144,7 @@ public class IterativeRobot extends RobotBase {
                     didTeleopPeriodic = true;
                 }
             }
+        	m_ds.waitForData();
         }
     }
 
@@ -152,11 +153,7 @@ public class IterativeRobot extends RobotBase {
      * Call the periodic functions whenever a packet is received from the Driver Station, or about every 20ms.
      */
     private boolean nextPeriodReady() {
-        // TODO: return m_ds.isNewControlData();
-        try {
-            Thread.sleep(20); // TODO: Find a better solution. This one is way too hacky!
-        } catch (InterruptedException ex) {}
-    	return true;
+    	return m_ds.isNewControlData();
     }
 
     /* ----------- Overridable initialization code -----------------*/

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/PIDController.java

@@ -6,8 +6,6 @@
 /*----------------------------------------------------------------------------*/
 package edu.wpi.first.wpilibj;
 
-import java.util.TimerTask;
-
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.parsing.IUtility;
 import edu.wpi.first.wpilibj.tables.ITable;
@@ -44,7 +42,6 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
     private double m_period = kDefaultPeriod;
     PIDSource m_pidInput;
     PIDOutput m_pidOutput;
-    java.util.Timer m_controlLoop;
     private boolean m_freed = false;
     private boolean m_usingPercentTolerance;
 
@@ -89,8 +86,7 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         }
     }
 
-    private class PIDTask extends TimerTask {
-
+    private class PIDTask implements Runnable {
         private PIDController m_controller;
 
         public PIDTask(PIDController controller) {
@@ -101,10 +97,9 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         }
 
         public void run() {
-        	if(!m_freed){
+        	while (!m_controller.m_freed) {
         		m_controller.calculate();
-        	} else {
-        		cancel();
+        		Timer.delay(m_controller.m_period);
         	}
         }
     }
@@ -131,9 +126,6 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
             throw new NullPointerException("Null PIDOutput was given");
         }
 
-        m_controlLoop = new java.util.Timer();
-
-
         m_P = Kp;
         m_I = Ki;
         m_D = Kd;
@@ -143,7 +135,7 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
         m_pidOutput = output;
         m_period = period;
 
-        m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));
+        new Thread(new PIDTask(this)).start();
 
         instances++;
         m_tolerance = new NullTolerance();
@@ -196,10 +188,8 @@ public class PIDController implements IUtility, LiveWindowSendable, Controller {
     public void free() {
     	m_freed = true;
     	if(this.table!=null) table.removeTableListener(listener);
-        m_controlLoop.cancel();
         m_pidInput = null;
     	m_pidOutput = null;
-        m_controlLoop = null;
     }
 
     /**

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Timer.java

@@ -7,7 +7,12 @@
 
 package edu.wpi.first.wpilibj;
 
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.SubscriberCallback;
+
 import edu.wpi.first.wpilibj.parsing.IUtility;
+import edu.wpi.first.wpilibj.simulation.MainNode;
+import gazebo.msgs.GzFloat64.Float64;
 
 /**
  * Timer objects measure accumulated time in milliseconds.
@@ -21,6 +26,19 @@ public class Timer implements IUtility {
     private long m_startTime;
     private double m_accumulatedTime;
     private boolean m_running;
+    private static double simTime;
+    private static Object time_notifier = new Object();
+    static {
+    	MainNode.subscribe("time", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override
+				public void callback(Float64 msg) {
+					simTime = msg.getData();
+					synchronized(time_notifier) { time_notifier.notifyAll(); } // Ew, this is nested too deep... Refactor?
+				}
+			}
+		);
+    }
 
     /**
      * Pause the thread for a specified time. Pause the execution of the
@@ -32,10 +50,17 @@ public class Timer implements IUtility {
      * @param seconds Length of time to pause
      */
     public static void delay(final double seconds) {
-        try {
-            Thread.sleep((long) (seconds * 1e3));
-        } catch (final InterruptedException e) {
-        }
+    	final double start = simTime;
+    	
+    	while ((simTime - start) < seconds) {
+    		synchronized(time_notifier) {
+    			try {
+    				time_notifier.wait(); // Block until time progresses
+    			} catch (InterruptedException e) {
+    				e.printStackTrace();
+    			}
+    		}
+		}
     }
 
     /**
@@ -46,7 +71,7 @@ public class Timer implements IUtility {
      * @return Robot running time in microseconds.
      */
     public static long getUsClock() {
-        return System.nanoTime() / 1000;
+        return (long) (simTime * 1e6);
     }
 
     /**
@@ -57,7 +82,7 @@ public class Timer implements IUtility {
      * @return Robot running time in milliseconds.
      */
     static long getMsClock() {
-        return System.currentTimeMillis();
+        return (long) (simTime * 1e3);
     }
 
     /**
@@ -67,7 +92,7 @@ public class Timer implements IUtility {
      * @return Robot running time in seconds.
      */
     public static double getFPGATimestamp() {
-        return System.currentTimeMillis() / 1000.0;
+        return simTime;
     }
 
     /**