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

simulation/SimDS/src/main/java/edu/wpi/first/wpilibj/simulation/ds/Main.java

@@ -1,8 +1,16 @@
 package edu.wpi.first.wpilibj.simulation.ds;
 
+import gazebo.msgs.GzFloat64.Float64;
+
+import org.gazebosim.transport.Msgs;
 import org.gazebosim.transport.Node;
+import org.gazebosim.transport.Subscriber;
+import org.gazebosim.transport.SubscriberCallback;
 
 public class Main {
+	private static double simTime = 0;
+	private static Subscriber<Float64> sub;
+	
 	public static void main(String args[]) {
 		Node node = new Node("frc");
 		try {
@@ -18,19 +26,35 @@ public class Main {
 		DS ds = new DS(provider);
 		ds.advertise(node);
 
+    	sub = node.subscribe("time", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override
+				public void callback(Float64 msg) {
+					simTime = msg.getData();
+					synchronized(sub) {
+						sub.notifyAll();
+					}
+				}
+			}
+		);
+
 		while (true) {
-			ds.publish();
+	    	final double start = simTime;
 			for (int i = 0; i < provider.getJoysticks().size(); i++) {
 				ISimJoystick joystick = provider.getJoysticks().get(i);
 				joystick.advertise(node, i+1);
 				joystick.publish();
 			}
-			
-			try {
-				Thread.sleep(19);
-			} catch (InterruptedException e) {
-				// TODO Auto-generated catch block
-				e.printStackTrace();
+			ds.publish();
+
+	    	while ((simTime - start) < 0.020 /*20ms*/) {
+	    		synchronized(sub) {
+	    			try {
+	    				sub.wait(); // Block until time progresses
+	    			} catch (InterruptedException e) {
+	    				e.printStackTrace();
+	    			}
+	    		}
 			}
 		}
 	}