Initial commit of the WPILib simulation support in an alpha quality state.

Fixes to deal with the switch to .hpp files in the HAL and other misc problems due to rebasing.

Added Omar's changes to the compressor interface

Fixes to make C++ plugin compile on linux.

Added import of the WPILibSim code from the graduate class. It shows up as wpilibJavaSim to follow the convention set by wpilibJava, wpilibJavaJNI and wpilibJavaFinal.

Fixed wpilibJavaSim artifactId to mirror the new convention.

Modified the build of the java plugin to pull in the simulation dependencies.

Added stacktrace printing.

Fixed support for creating projects.

Added support for the isReal() and isSimulation() methods along with the AnalogPotentiometer object to support simulating GearsBot.

Added support for a "WPILib Simulate" button.

Added GearsBot to the built in examples.

Added support for specifying the world file during project creation and switched the default from BluntObjectBot to GearsBot.

Removed unused import.

Added file browser for world files.

Added support for debugging in simulation.

Change simulate icon to be a Gazebo icon.

Switched over to the gazebo messaging system.

Updated location of default world file.

Reverted cmake change.

Fixed bug in WPILibJSim, added better logging and cleaned up code.

Made the frc_gazebo_plugin build using raw cmake instead of catkin, breaking the final ROS dependencies.

Added installation to frc_gazebo_plugin Makefile.

Fixed running of simulation to actually use frcsim.

Initial commit of simulation library for C++. Has the minimal subset of features necessary for having a Simple Robot run in teleoperated mode.

Added notes for generating protobuf messages.

Import of the debuild process into the main repository.

Moved frc_gazebo_plugin under simulation and removed the gazebo folder.

Updated the gazebo plugin to remove excessive printing and limit motor signal to [-1,1].

Updated WPILibJSim to support latching messages and to sleep for 20ms in iterative robot.

Reduced delay between starting frcsim and the users program to 1 second.

Updated GearsBot example.

Fixed a few minor issues for demoable state.

Added simulator support for Victors, Jaguars and Talons.

Added NetworkTables, SmartDashboard and LiveWindow to the simulator.

Added AnalogPotentiometer for simulation.

Added support for simulating encoders.

Added simulation support for Gyro.

Added IterativeRobot, Fixed Timers, Notifiers, PIDControllers and other minor fixes + cleanup.

Added RobotDrive support to simulation.

Separated out JavaGazebo so that SimDS will be able to reuse it.

Separated out SimDS into its own application..

Fixes so that the SimDS is distributed and runs properly for Java with the eclipse plugins.

Added DriverStation support to WPILibCSim

Cleanup of DriverStation, WaitUntilCommand and AnalogPotentiometer for WPILibCSim.

Cleanup of includes for WPILibCSim

Added AnalogPotentiometer to the real WPILibC.

Added AnalogPotentiometer to the real WPILibC.

Added GearsBot example to C++ eclipse plugin.

WPILibCSim fixes to work with launching from the plugin.

Package libwpilibsim in a deb file.

Added includes to plugin distribution.

Added support for external-limit-switches to Gazebo, Java and C++.

Added support for Gazebo Rangefinders and Analog channels to read their values in C++ and Java.

Added support for internal limit switches.

Updated GearsBot programs to use limit switches + range finders.

Added disabling of motors when robot is disabled to more closely mimic the real robot.

Fixes to deal with the switch to .hpp files in the HAL and other misc problems due to rebasing.

Change-Id: I624c5f4d0f28282616a7c92083575bf68adcdce2

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

@@ -0,0 +1,156 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimFloatInput;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * Analog channel class.
+ */
+public class AnalogChannel extends SensorBase implements PIDSource,
+		LiveWindowSendable {
+
+	private SimFloatInput m_impl;
+	private int m_moduleNumber, m_channel;
+
+	/**
+	 * Construct an analog channel on the default module.
+	 * 
+	 * @param channel
+	 *            The channel number to represent.
+	 */
+	public AnalogChannel(final int channel) {
+		this(getDefaultAnalogModule(), channel);
+	}
+
+	/**
+	 * Construct an analog channel on a specified module.
+	 * 
+	 * @param moduleNumber
+	 *            The digital module to use (1 or 2).
+	 * @param channel
+	 *            The channel number to represent.
+	 */
+	public AnalogChannel(final int moduleNumber, final int channel) {
+		m_channel = channel;
+		m_moduleNumber = moduleNumber;
+		m_impl = new SimFloatInput("simulator/analog/"+moduleNumber+"/"+channel);
+
+		LiveWindow.addSensor("Analog", moduleNumber, channel, this);
+	}
+
+	/**
+	 * Channel destructor.
+	 */
+	public void free() {
+		m_channel = 0;
+		m_moduleNumber = 0;
+	}
+
+	/**
+	 * Get a scaled sample straight from this channel on the module. The value
+	 * is scaled to units of Volts using the calibrated scaling data from
+	 * getLSBWeight() and getOffset().
+	 * 
+	 * @return A scaled sample straight from this channel on the module.
+	 */
+	public double getVoltage() {
+		return m_impl.get();
+	}
+
+	/**
+	 * Get a scaled sample from the output of the oversample and average engine
+	 * for this channel. The value is scaled to units of Volts using the
+	 * calibrated scaling data from getLSBWeight() and getOffset(). Using
+	 * oversampling will cause this value to be higher resolution, but it will
+	 * update more slowly. Using averaging will cause this value to be more
+	 * stable, but it will update more slowly.
+	 * 
+	 * @return A scaled sample from the output of the oversample and average
+	 *         engine for this channel.
+	 */
+	public double getAverageVoltage() {
+		return getVoltage();
+	}
+
+	/**
+	 * Get the channel number.
+	 * 
+	 * @return The channel number.
+	 */
+	public int getChannel() {
+		return m_channel;
+	}
+
+	/**
+	 * Gets the number of the analog module this channel is on.
+	 * 
+	 * @return The module number of the analog module this channel is on.
+	 */
+	public int getModuleNumber() {
+		return m_moduleNumber;
+	}
+
+	/*
+	 * Get the average value for use with PIDController
+	 * 
+	 * @return the average value
+	 */
+	public double pidGet() {
+		return getAverageVoltage();
+	}
+
+	/*
+	 * Live Window code, only does anything if live window is activated.
+	 */
+	public String getSmartDashboardType() {
+		return "Analog Input";
+	}
+
+	private ITable m_table;
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void initTable(ITable subtable) {
+		m_table = subtable;
+		updateTable();
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void updateTable() {
+		if (m_table != null) {
+			m_table.putNumber("Value", getAverageVoltage());
+		}
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public ITable getTable() {
+		return m_table;
+	}
+
+	/**
+	 * Analog Channels don't have to do anything special when entering the
+	 * LiveWindow. {@inheritDoc}
+	 */
+	public void startLiveWindowMode() {
+	}
+
+	/**
+	 * Analog Channels don't have to do anything special when exiting the
+	 * LiveWindow. {@inheritDoc}
+	 */
+	public void stopLiveWindowMode() {
+	}
+}

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

@@ -0,0 +1,94 @@
+package edu.wpi.first.wpilibj;
+import edu.wpi.first.wpilibj.interfaces.Potentiometer;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimFloatInput;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * A class for reading analog potentiometers.
+ * 
+ * @author Alex Henning
+ */
+public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
+	private int module, channel;
+	private SimFloatInput impl;
+
+    /**
+     * Common initialization code called by all constructors.
+     */
+    private void initPot(final int slot, final int channel, double scale, double offset) {
+    	this.module = slot;
+    	this.channel = channel;
+    	impl = new SimFloatInput("simulator/analog/"+slot+"/"+channel);
+    }
+    
+    public AnalogPotentiometer(final int slot, final int channel, double scale, double offset) {
+    	initPot(slot, channel, scale, offset);
+    }
+
+    public AnalogPotentiometer(final int channel, double scale, double offset) {
+    	initPot(1, channel, scale, offset);
+    }
+
+    public AnalogPotentiometer(final int channel, double scale) {
+    	initPot(1, channel, scale, 0);
+    }
+
+    public AnalogPotentiometer(final int channel) {
+    	initPot(1, channel, 1, 0);
+    }
+
+	@Override
+	public double get() {
+		return impl.get();
+	}
+    
+	@Override
+	public double pidGet() {
+		return get();
+	}
+    
+    /*
+     * Live Window code, only does anything if live window is activated.
+     */
+    public String getSmartDashboardType(){
+        return "Analog Input";
+    }
+    private ITable m_table;
+    
+    /**
+     * {@inheritDoc}
+     */
+    public void initTable(ITable subtable) {
+        m_table = subtable;
+        updateTable();
+    }
+    
+    /**
+     * {@inheritDoc}
+     */
+    public void updateTable() {
+        if (m_table != null) {
+            m_table.putNumber("Value", get());
+        }
+    }
+    
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable(){
+        return m_table;
+    }
+    
+    /**
+     * Analog Channels don't have to do anything special when entering the LiveWindow.
+     * {@inheritDoc}
+     */
+    public void startLiveWindowMode() {}
+    
+    /**
+     * Analog Channels don't have to do anything special when exiting the LiveWindow.
+     * {@inheritDoc}
+     */
+    public void stopLiveWindowMode() {}
+}

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

@@ -0,0 +1,25 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package edu.wpi.first.wpilibj;
+
+/**
+ * An interface for controllers. Controllers run control loops, the most command
+ * are PID controllers and there variants, but this includes anything that is
+ * controlling an actuator in a separate thread.
+ *
+ * @author alex
+ */
+interface Controller {
+    /**
+     * Allows the control loop to run.
+     */
+    public void enable();
+
+    /**
+     * Stops the control loop from running until explicitly re-enabled by calling
+     * {@link enable()}.
+     */
+    public void disable();
+}

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

@@ -0,0 +1,91 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for counting the number of ticks on a digital input channel.
+ * Encoders, Gear tooth sensors, and counters should all subclass this so it can be used to
+ * build more advanced classes for control and driving.
+ */
+public interface CounterBase {
+
+    /**
+     * The number of edges for the counterbase to increment or decrement on
+     */
+    public static class EncodingType {
+
+        /**
+         * The integer value representing this enumeration
+         */
+        public final int value;
+        static final int k1X_val = 0;
+        static final int k2X_val = 1;
+        static final int k4X_val = 2;
+        /**
+         * Count only the rising edge
+         */
+        public static final EncodingType k1X = new EncodingType(k1X_val);
+        /**
+         * Count both the rising and falling edge
+         */
+        public static final EncodingType k2X = new EncodingType(k2X_val);
+        /**
+         * Count rising and falling on both channels
+         */
+        public static final EncodingType k4X = new EncodingType(k4X_val);
+
+        private EncodingType(int value) {
+            this.value = value;
+        }
+    }
+
+    /**
+     * Start the counter
+     */
+    public void start();
+
+    /**
+     * Get the count
+     * @return the count
+     */
+    int get();
+
+    /**
+     * Reset the count to zero
+     */
+    void reset();
+
+    /**
+     * Stop counting
+     */
+    void stop();
+
+    /**
+     * Get the time between the last two edges counted
+     * @return the time beteween the last two ticks in seconds
+     */
+    double getPeriod();
+
+    /**
+     * Set the maximum time between edges to be considered stalled
+     * @param maxPeriod the maximum period in seconds
+     */
+    void setMaxPeriod(double maxPeriod);
+
+    /**
+     * Determine if the counter is not moving
+     * @return true if the counter has not changed for the max period
+     */
+    boolean getStopped();
+
+    /**
+     * Determine which direction the counter is going
+     * @return true for one direction, false for the other
+     */
+    boolean getDirection();
+}

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

@@ -0,0 +1,104 @@
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimDigitalInput;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+public class DigitalInput implements LiveWindowSendable {
+	private SimDigitalInput impl;
+	private int m_moduleNumber, m_channel;
+
+	/**
+	 * Create an instance of a Digital Input class. Creates a digital input
+	 * given a channel and uses the default module.
+	 * 
+	 * @param channel
+	 *            the port for the digital input
+	 */
+	public DigitalInput(int channel) {
+		this(1, channel);
+	}
+
+	/**
+	 * Create an instance of a Digital Input class. Creates a digital input
+	 * given an channel and module.
+	 * 
+	 * @param moduleNumber
+	 *            The number of the digital module to use for this input
+	 * @param channel
+	 *            the port for the digital input
+	 */
+	public DigitalInput(int moduleNumber, int channel) {
+		impl = new SimDigitalInput("simulator/dio/"+moduleNumber+"/"+channel);
+		m_moduleNumber = moduleNumber;
+		m_channel = channel;
+	}
+
+	/**
+	 * Get the value from a digital input channel. Retrieve the value of a
+	 * single digital input channel from the FPGA.
+	 * 
+	 * @return the stats of the digital input
+	 */
+	public boolean get() {
+		return impl.get();
+	}
+
+	/**
+	 * Get the channel of the digital input
+	 * 
+	 * @return The GPIO channel number that this object represents.
+	 */
+	public int getChannel() {
+		return m_channel;
+	}
+
+	public boolean getAnalogTriggerForRouting() {
+		return false;
+	}
+
+	/*
+	 * Live Window code, only does anything if live window is activated.
+	 */
+	public String getSmartDashboardType() {
+		return "Digital Input";
+	}
+
+	private ITable m_table;
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void initTable(ITable subtable) {
+		m_table = subtable;
+		updateTable();
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void updateTable() {
+		if (m_table != null) {
+			m_table.putBoolean("Value", get());
+		}
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public ITable getTable() {
+		return m_table;
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void startLiveWindowMode() {
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void stopLiveWindowMode() {
+	}
+}

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

@@ -0,0 +1,365 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.simulation.MainNode;
+import gazebo.msgs.GzDriverStation;
+import gazebo.msgs.GzDriverStation.DriverStation.State;
+import gazebo.msgs.GzJoystick.Joystick;
+
+import org.gazebosim.transport.SubscriberCallback;
+
+/**
+ * Provide access to the network communication data to / from the Driver Station.
+ */
+public class DriverStation {
+    /**
+     * Slot for the analog module to read the battery
+     */
+    public static final int kBatterySlot = 1;
+    /**
+     * Analog channel to read the battery
+     */
+    public static final int kBatteryChannel = 8;
+    /**
+     * Number of Joystick Ports
+     */
+    public static final int kJoystickPorts = 4;
+    /**
+     * Number of Joystick Axes
+     */
+    public static final int kJoystickAxes = 6;
+
+    /**
+     * The robot alliance that the robot is a part of
+     */
+    public static class Alliance {
+
+        /** The integer value representing this enumeration. */
+        public final int value;
+        /** The Alliance name. */
+        public final String name;
+        public static final int kRed_val = 0;
+        public static final int kBlue_val = 1;
+        public static final int kInvalid_val = 2;
+        /** alliance: Red */
+        public static final Alliance kRed = new Alliance(kRed_val, "Red");
+        /** alliance: Blue */
+        public static final Alliance kBlue = new Alliance(kBlue_val, "Blue");
+        /** alliance: Invalid */
+        public static final Alliance kInvalid = new Alliance(kInvalid_val, "invalid");
+
+        private Alliance(int value, String name) {
+            this.value = value;
+            this.name = name;
+        }
+    } /* Alliance */
+
+
+    private static DriverStation instance = new DriverStation();
+    private final Object m_dataSem;
+    private boolean m_userInDisabled = false;
+    private boolean m_userInAutonomous = false;
+    private boolean m_userInTeleop = false;
+    private boolean m_userInTest = false;
+    private boolean m_newControlData;
+    private GzDriverStation.DriverStation state;
+    private Joystick joysticks[] = new Joystick[4];
+
+    /**
+     * Gets an instance of the DriverStation
+     *
+     * @return The DriverStation.
+     */
+    public static DriverStation getInstance() {
+        return DriverStation.instance;
+    }
+
+	/**
+     * DriverStation constructor.
+     *
+     * The single DriverStation instance is created statically with the
+     * instance static member variable.
+     */
+    protected DriverStation() {
+        m_dataSem = new Object();
+
+        MainNode.subscribe("ds/state", GzDriverStation.DriverStation.getDefaultInstance(),
+    		new SubscriberCallback<GzDriverStation.DriverStation>() {
+				@Override public void callback(GzDriverStation.DriverStation msg) {
+					synchronized (m_dataSem) {
+						state = msg;
+					}
+				}
+			}
+        );
+        
+        for (int i = 1; i <= 4; i++) {
+        	final int j = i;
+        	MainNode.subscribe("ds/joysticks/"+i, Joystick.getDefaultInstance(),
+        		new SubscriberCallback<Joystick>() {
+					@Override public void callback(Joystick msg) {
+						synchronized (m_dataSem) {
+							joysticks[j] = msg;
+						}
+					}
+				}
+			);
+        }
+    }
+
+    /**
+     * Wait for new data from the driver station.
+     */
+    public void waitForData() {
+        waitForData(0);
+    }
+
+    /**
+     * Wait for new data or for timeout, which ever comes first.  If timeout is
+     * 0, wait for new data only.
+     *
+     * @param timeout The maximum time in milliseconds to wait.
+     */
+    public void waitForData(long timeout) {
+        synchronized (m_dataSem) {
+            try {
+                m_dataSem.wait(timeout);
+            } catch (InterruptedException ex) {
+            }
+        }
+    }
+
+    /**
+     * Read the battery voltage from the specified AnalogChannel.
+     *
+     * This accessor assumes that the battery voltage is being measured
+     * through the voltage divider on an analog breakout.
+     *
+     * @return The battery voltage.
+     */
+    public double getBatteryVoltage() {
+    	return 12.0; // 12 volts all the time!
+    }
+
+    /**
+     * Get the value of the axis on a joystick.
+     * This depends on the mapping of the joystick connected to the specified port.
+     *
+     * @param stick The joystick to read.
+     * @param axis The analog axis value to read from the joystick.
+     * @return The value of the axis on the joystick.
+     */
+    public double getStickAxis(int stick, int axis) {
+        if (axis < 1 || axis > kJoystickAxes || joysticks[stick] == null) {
+            return 0.0;    		
+    	}
+    	return joysticks[stick].getAxes(axis - 1);
+    }
+
+    /**
+     * The state of the buttons on the joystick.
+     * 12 buttons (4 msb are unused) from the joystick.
+     *
+     * @param stick The joystick to read.
+     * @return The state of the buttons on the joystick.
+     */
+    public boolean getStickButton(int stick, int button) {
+    	if (joysticks[stick] == null) {
+            return false;    		
+    	}
+    	return joysticks[stick].getButtons(button - 1);
+    }
+
+    /**
+     * Get an analog voltage from the Driver Station.
+     * The analog values are returned as voltage values for the Driver Station analog inputs.
+     * These inputs are typically used for advanced operator interfaces consisting of potentiometers
+     * or resistor networks representing values on a rotary switch.
+     *
+     * @param channel The analog input channel on the driver station to read from. Valid range is 1 - 4.
+     * @return The analog voltage on the input.
+     */
+    public double getAnalogIn(final int channel) {
+    	throw new RuntimeException("The simulated DriverStation doesn't have analog inputs");
+    }
+
+    /**
+     * Get values from the digital inputs on the Driver Station.
+     * Return digital values from the Drivers Station. These values are typically used for buttons
+     * and switches on advanced operator interfaces.
+     * @param channel The digital input to get. Valid range is 1 - 8.
+     * @return The value of the digital input
+     */
+    public boolean getDigitalIn(final int channel) {
+    	throw new RuntimeException("The simulated DriverStation doesn't have digital inputs");
+    }
+
+    /**
+     * Set a value for the digital outputs on the Driver Station.
+     *
+     * Control digital outputs on the Drivers Station. These values are typically used for
+     * giving feedback on a custom operator station such as LEDs.
+     *
+     * @param channel The digital output to set. Valid range is 1 - 8.
+     * @param value The state to set the digital output.
+     */
+    public void setDigitalOut(final int channel, final boolean value) {
+    	throw new RuntimeException("The simulated DriverStation doesn't have digital outputs");
+    }
+
+    /**
+     * Get a value that was set for the digital outputs on the Driver Station.
+     * @param channel The digital ouput to monitor. Valid range is 1 through 8.
+     * @return A digital value being output on the Drivers Station.
+     */
+    public boolean getDigitalOut(final int channel) {
+    	throw new RuntimeException("The simulated DriverStation doesn't have digital outputs");
+    }
+
+    /**
+     * Gets a value indicating whether the Driver Station requires the
+     * robot to be enabled.
+     *
+     * @return True if the robot is enabled, false otherwise.
+     */
+    public boolean isEnabled() {
+    	return state != null ? state.getEnabled() : false;
+    }
+
+    /**
+     * Gets a value indicating whether the Driver Station requires the
+     * robot to be disabled.
+     *
+     * @return True if the robot should be disabled, false otherwise.
+     */
+    public boolean isDisabled() {
+        return !isEnabled();
+    }
+
+    /**
+     * Gets a value indicating whether the Driver Station requires the
+     * robot to be running in autonomous mode.
+     *
+     * @return True if autonomous mode should be enabled, false otherwise.
+     */
+    public boolean isAutonomous() {
+    	return state != null ? state.getState() == State.AUTO : false;
+    }
+
+    /**
+     * Gets a value indicating whether the Driver Station requires the
+     * robot to be running in test mode.
+     * @return True if test mode should be enabled, false otherwise.
+     */
+    public boolean isTest() {
+    	return state != null ? state.getState() == State.TEST : false;
+    }
+
+    /**
+     * Gets a value indicating whether the Driver Station requires the
+     * robot to be running in operator-controlled mode.
+     *
+     * @return True if operator-controlled mode should be enabled, false otherwise.
+     */
+    public boolean isOperatorControl() {
+        return !(isAutonomous() || isTest());
+    }
+
+    /**
+     * Has a new control packet from the driver station arrived since the last time this function was called?
+     * @return True if the control data has been updated since the last call.
+     */
+    public synchronized boolean isNewControlData() {
+        boolean result = m_newControlData;
+        m_newControlData = false;
+        return result;
+    }
+
+    /**
+     * Get the current alliance from the FMS
+     * @return the current alliance
+     */
+    public Alliance getAlliance() {
+        switch ('I') { // TODO: Always invalid
+        case 'R':
+            return Alliance.kRed;
+        case 'B':
+            return Alliance.kBlue;
+        default:
+            return Alliance.kInvalid;
+        }
+    }
+
+    /**
+     * Gets the location of the team's driver station controls.
+     *
+     * @return the location of the team's driver station controls: 1, 2, or 3
+     */
+    public int getLocation() {
+        return -1; // TODO: always -1
+    }
+
+    /**
+     * Return the team number that the Driver Station is configured for
+     * @return The team number
+     */
+    public int getTeamNumber() {
+        return 348; // TODO
+    }
+    
+    /**
+     * Is the driver station attached to a Field Management System?
+     * Note: This does not work with the Blue DS.
+     * @return True if the robot is competing on a field being controlled by a Field Management System
+     */
+    public boolean isFMSAttached() {
+        return false;
+    }
+
+    /**
+     * Return the approximate match time
+     * The FMS does not currently send the official match time to the robots
+     * This returns the time since the enable signal sent from the Driver Station
+     * At the beginning of autonomous, the time is reset to 0.0 seconds
+     * At the beginning of teleop, the time is reset to +15.0 seconds
+     * If the robot is disabled, this returns 0.0 seconds
+     * Warning: This is not an official time (so it cannot be used to argue with referees)
+     * @return Match time in seconds since the beginning of autonomous
+     */
+    public double getMatchTime() {
+    	throw new RuntimeException("The simulated DriverStation doesn't support match times"); // TODO:
+    }
+
+    /** Only to be used to tell the Driver Station what code you claim to be executing
+     *   for diagnostic purposes only
+     * @param entering If true, starting disabled code; if false, leaving disabled code */
+    public void InDisabled(boolean entering) {
+        m_userInDisabled=entering;
+    }
+
+    /** Only to be used to tell the Driver Station what code you claim to be executing
+    *   for diagnostic purposes only
+     * @param entering If true, starting autonomous code; if false, leaving autonomous code */
+    public void InAutonomous(boolean entering) {
+        m_userInAutonomous=entering;
+    }
+
+    /** Only to be used to tell the Driver Station what code you claim to be executing
+    *   for diagnostic purposes only
+     * @param entering If true, starting teleop code; if false, leaving teleop code */
+    public void InOperatorControl(boolean entering) {
+        m_userInTeleop=entering;
+    }
+
+    /** Only to be used to tell the Driver Station what code you claim to be executing
+     *   for diagnostic purposes only
+     * @param entering If true, starting test code; if false, leaving test code */
+    public void InTest(boolean entering) {
+        m_userInTeleop = entering;
+    }
+}

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

@@ -0,0 +1,541 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimEncoder;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.util.BoundaryException;
+
+/**
+ * Class to read quad encoders.
+ * Quadrature encoders are devices that count shaft rotation and can sense direction. The output of
+ * the QuadEncoder class is an integer that can count either up or down, and can go negative for
+ * reverse direction counting. When creating QuadEncoders, a direction is supplied that changes the
+ * sense of the output to make code more readable if the encoder is mounted such that forward movement
+ * generates negative values. Quadrature encoders have two digital outputs, an A Channel and a B Channel
+ * that are out of phase with each other to allow the FPGA to do direction sensing.
+ */
+public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveWindowSendable {
+    private int m_index;
+    private double m_distancePerPulse;		// distance of travel for each encoder tick
+    private EncodingType m_encodingType = EncodingType.k4X;
+    private boolean m_allocatedA;
+    private boolean m_allocatedB;
+    private boolean m_allocatedI;
+    private PIDSourceParameter m_pidSource;
+    private SimEncoder impl;
+
+    /**
+     * Common initialization code for Encoders.
+     * This code allocates resources for Encoders and is common to all constructors.
+     * @param reverseDirection If true, counts down instead of up (this is all relative)
+     * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+     * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+     * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+     * a counter object will be used and the returned value will either exactly match the spec'd count
+     * or be double (2x) the spec'd count.
+     */
+    private void initEncoder(int aSlot, int aChannel,
+            int bSlot, int bChannel, boolean reverseDirection) {
+
+    	LiveWindow.addSensor("Encoder", aSlot, aChannel, this);
+        
+        if ((bSlot < aSlot) || ((bSlot == aSlot) && (bChannel < aChannel))) { // Swap ports
+        	int slot = bSlot;
+        	int channel = bChannel;
+        	bSlot = aSlot;
+        	bChannel = aChannel;
+        	aSlot = slot;
+        	aChannel = channel;
+        }
+    	impl = new SimEncoder("simulator/dio/"+aSlot+"/"+aChannel+"/"+bSlot+"/"+bChannel);
+    	setDistancePerPulse(1);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b modules and channels fully specified.
+     * @param aSlot The a channel digital input module.
+     * @param aChannel The a channel digital input channel.
+     * @param bSlot The b channel digital input module.
+     * @param bChannel The b channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    public Encoder(final int aSlot, final int aChannel,
+            final int bSlot, final int bChannel,
+            boolean reverseDirection) {
+        m_allocatedA = true;
+        m_allocatedB = true;
+        m_allocatedI = false;
+        initEncoder(aSlot, aChannel, bSlot, bChannel, reverseDirection);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b modules and channels fully specified.
+     * @param aSlot The a channel digital input module.
+     * @param aChannel The a channel digital input channel.
+     * @param bSlot The b channel digital input module.
+     * @param bChannel The b channel digital input channel.
+     */
+    public Encoder(final int aSlot, final int aChannel,
+            final int bSlot, final int bChannel) {
+        this(aSlot, aChannel, bSlot, bChannel, false);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b modules and channels fully specified.
+     * @param aSlot The a channel digital input module.
+     * @param aChannel The a channel digital input channel.
+     * @param bSlot The b channel digital input module.
+     * @param bChannel The b channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+     * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+     * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+     * a counter object will be used and the returned value will either exactly match the spec'd count
+     * or be double (2x) the spec'd count.
+     */
+    public Encoder(final int aSlot, final int aChannel,
+            final int bSlot, final int bChannel,
+            boolean reverseDirection, final EncodingType encodingType) {
+        m_allocatedA = true;
+        m_allocatedB = true;
+        m_allocatedI = false;
+        if (encodingType == null)
+            throw new NullPointerException("Given encoding type was null");
+        m_encodingType = encodingType;
+        initEncoder(aSlot, aChannel, bSlot, bChannel, reverseDirection);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b modules and channels fully specified.
+     * Using the index pulse forces 4x encoding.
+     * @param aSlot The a channel digital input module.
+     * @param aChannel The a channel digital input channel.
+     * @param bSlot The b channel digital input module.
+     * @param bChannel The b channel digital input channel.
+     * @param indexSlot The index channel digital input module.
+     * @param indexChannel The index channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    public Encoder(final int aSlot, final int aChannel,
+            final int bSlot, final int bChannel, final int indexSlot,
+            final int indexChannel,
+            boolean reverseDirection) {
+    	throw new UnsupportedOperationException("Simulation doesn't currently support indexed encoders.");
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b modules and channels fully specified.
+     * Using the index pulse forces 4x encoding.
+     * @param aSlot The a channel digital input module.
+     * @param aChannel The a channel digital input channel.
+     * @param bSlot The b channel digital input module.
+     * @param bChannel The b channel digital input channel.
+     * @param indexSlot The index channel digital input module.
+     * @param indexChannel The index channel digital input channel.
+     */
+    public Encoder(final int aSlot, final int aChannel,
+            final int bSlot, final int bChannel, final int indexSlot,
+            final int indexChannel) {
+    	throw new UnsupportedOperationException("Simulation doesn't currently support indexed encoders.");
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels assuming the default module.
+     * @param aChannel The a channel digital input channel.
+     * @param bChannel The b channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    public Encoder(final int aChannel, final int bChannel, boolean reverseDirection) {
+        m_allocatedA = true;
+        m_allocatedB = true;
+        m_allocatedI = false;
+        initEncoder(1, aChannel, 1, bChannel, reverseDirection);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels assuming the default module.
+     * @param aChannel The a channel digital input channel.
+     * @param bChannel The b channel digital input channel.
+     */
+    public Encoder(final int aChannel, final int bChannel) {
+        this(aChannel, bChannel, false);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels assuming the default module.
+     * @param aChannel The a channel digital input channel.
+     * @param bChannel The b channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+     * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+     * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+     * a counter object will be used and the returned value will either exactly match the spec'd count
+     * or be double (2x) the spec'd count.
+     */
+    public Encoder(final int aChannel, final int bChannel, boolean reverseDirection, final EncodingType encodingType) {
+        m_allocatedA = true;
+        m_allocatedB = true;
+        m_allocatedI = false;
+        if (encodingType == null)
+            throw new NullPointerException("Given encoding type was null");
+        m_encodingType = encodingType;
+        initEncoder(1, aChannel, 1, bChannel, reverseDirection);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels assuming the default module.
+     * Using an index pulse forces 4x encoding
+     * @param aChannel The a channel digital input channel.
+     * @param bChannel The b channel digital input channel.
+     * @param indexChannel The index channel digital input channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    public Encoder(final int aChannel, final int bChannel, final int indexChannel, boolean reverseDirection) {
+    	throw new UnsupportedOperationException("Simulation doesn't currently support indexed encoders.");
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels assuming the default module.
+     * Using an index pulse forces 4x encoding
+     * @param aChannel The a channel digital input channel.
+     * @param bChannel The b channel digital input channel.
+     * @param indexChannel The index channel digital input channel.
+     */
+    public Encoder(final int aChannel, final int bChannel, final int indexChannel) {
+        this(aChannel, bChannel, indexChannel, false);
+    }
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+     * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+     * and assume that they already are counted.
+     * @param aSource The source that should be used for the a channel.
+     * @param bSource the source that should be used for the b channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    // Not Supported: public Encoder(DigitalSource aSource, DigitalSource bSource, boolean reverseDirection) {
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+     * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+     * and assume that they already are counted.
+     * @param aSource The source that should be used for the a channel.
+     * @param bSource the source that should be used for the b channel.
+     */
+    // Not Supported: public Encoder(DigitalSource aSource, DigitalSource bSource) {
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+     * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+     * and assume that they already are counted.
+     * @param aSource The source that should be used for the a channel.
+     * @param bSource the source that should be used for the b channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+     * selected, then an encoder FPGA object is used and the returned counts will be 4x the encoder
+     * spec'd value since all rising and falling edges are counted. If 1X or 2X are selected then
+     * a counter object will be used and the returned value will either exactly match the spec'd count
+     * or be double (2x) the spec'd count.
+     */
+    // Not Supported: public Encoder(DigitalSource aSource, DigitalSource bSource, boolean reverseDirection, final EncodingType encodingType) {
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+     * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+     * and assume that they already are counted.
+     * @param aSource The source that should be used for the a channel.
+     * @param bSource the source that should be used for the b channel.
+     * @param indexSource the source that should be used for the index channel.
+     * @param reverseDirection represents the orientation of the encoder and inverts the output values
+     * if necessary so forward represents positive values.
+     */
+    // Not Supported: public Encoder(DigitalSource aSource, DigitalSource bSource, DigitalSource indexSource, boolean reverseDirection) {
+
+    /**
+     * Encoder constructor.
+     * Construct a Encoder given a and b channels as digital inputs. This is used in the case
+     * where the digital inputs are shared. The Encoder class will not allocate the digital inputs
+     * and assume that they already are counted.
+     * @param aSource The source that should be used for the a channel.
+     * @param bSource the source that should be used for the b channel.
+     * @param indexSource the source that should be used for the index channel.
+     */
+    // Not Supported: public Encoder(DigitalSource aSource, DigitalSource bSource, DigitalSource indexSource) {
+
+    public void free() {}
+
+    /**
+     * Start the Encoder.
+     * Starts counting pulses on the Encoder device.
+     */
+    public void start() {
+    	impl.start();
+    }
+
+    /**
+     * Stops counting pulses on the Encoder device. The value is not changed.
+     */
+    public void stop() {
+    	impl.stop();
+    }
+
+    /**
+     * Gets the raw value from the encoder.
+     * The raw value is the actual count unscaled by the 1x, 2x, or 4x scale
+     * factor.
+     * @return Current raw count from the encoder
+     */
+    public int getRaw() {
+    	throw new UnsupportedOperationException("Simulation doesn't currently support raw values.");
+    }
+
+    /**
+     * Gets the current count.
+     * Returns the current count on the Encoder.
+     * This method compensates for the decoding type.
+     *
+     * @return Current count from the Encoder adjusted for the 1x, 2x, or 4x scale factor.
+     */
+    public int get() {
+    	throw new UnsupportedOperationException("Simulation doesn't support ticks.");
+    }
+
+    /**
+     * Reset the Encoder distance to zero.
+     * Resets the current count to zero on the encoder.
+     */
+    public void reset() {
+    	impl.reset();
+    }
+
+    /**
+     * Returns the period of the most recent pulse.
+     * Returns the period of the most recent Encoder pulse in seconds.
+     * This method compensates for the decoding type.
+     *
+     * @deprecated Use getRate() in favor of this method.  This returns unscaled periods and getRate() scales using value from setDistancePerPulse().
+     *
+     * @return Period in seconds of the most recent pulse.
+     */
+    public double getPeriod() {
+    	throw new UnsupportedOperationException("Simulation doesn't currently support deprecated methods.");
+    }
+
+    /**
+     * Sets the maximum period for stopped detection.
+     * Sets the value that represents the maximum period of the Encoder before it will assume
+     * that the attached device is stopped. This timeout allows users to determine if the wheels or
+     * other shaft has stopped rotating.
+     * This method compensates for the decoding type.
+     *
+     *
+     * @param maxPeriod The maximum time between rising and falling edges before the FPGA will
+     * report the device stopped. This is expressed in seconds.
+     */
+    public void setMaxPeriod(double maxPeriod) {
+    	// XXX: not implemented in simulation yet
+    	throw new UnsupportedOperationException("Simulation doesn't currently support this method.");
+    }
+
+    /**
+     * Determine if the encoder is stopped.
+     * Using the MaxPeriod value, a boolean is returned that is true if the encoder is considered
+     * stopped and false if it is still moving. A stopped encoder is one where the most recent pulse
+     * width exceeds the MaxPeriod.
+     * @return True if the encoder is considered stopped.
+     */
+    public boolean getStopped() {
+    	// XXX: not implemented in simulation yet
+    	throw new UnsupportedOperationException("Simulation doesn't currently support this method.");
+    }
+
+    /**
+     * The last direction the encoder value changed.
+     * @return The last direction the encoder value changed.
+     */
+    public boolean getDirection() {
+    	// XXX: not implemented in simulation yet
+    	throw new UnsupportedOperationException("Simulation doesn't currently support this method.");
+    }
+
+    /**
+     * Get the distance the robot has driven since the last reset.
+     *
+     * @return The distance driven since the last reset as scaled by the value from setDistancePerPulse().
+     */
+    public double getDistance() {
+    	return m_distancePerPulse * impl.getPosition();
+    }
+
+    /**
+     * Get the current rate of the encoder.
+     * Units are distance per second as scaled by the value from setDistancePerPulse().
+     *
+     * @return The current rate of the encoder.
+     */
+    public double getRate() {
+    	return m_distancePerPulse * impl.getVelocity();
+    }
+
+    /**
+     * Set the minimum rate of the device before the hardware reports it stopped.
+     *
+     * @param minRate The minimum rate.  The units are in distance per second as scaled by the value from setDistancePerPulse().
+     */
+    public void setMinRate(double minRate) {
+    	// XXX: not implemented in simulation yet
+    	throw new UnsupportedOperationException("Simulation doesn't currently support this method.");
+    }
+
+    /**
+     * Set the distance per pulse for this encoder.
+     * This sets the multiplier used to determine the distance driven based on the count value
+     * from the encoder.
+     * Do not include the decoding type in this scale.  The library already compensates for the decoding type.
+     * Set this value based on the encoder's rated Pulses per Revolution and
+     * factor in gearing reductions following the encoder shaft.
+     * This distance can be in any units you like, linear or angular.
+     *
+     * @param distancePerPulse The scale factor that will be used to convert pulses to useful units.
+     */
+    public void setDistancePerPulse(double distancePerPulse) {
+    	System.err.println("NOTE|WPILibJSim: Encoder.setDistancePerPulse() assumes 360 pulses per revolution in simulation.");
+    	m_distancePerPulse = distancePerPulse;
+    }
+
+    /**
+     * Set the direction sensing for this encoder.
+     * This sets the direction sensing on the encoder so that it could count in the correct
+     * software direction regardless of the mounting.
+     * @param reverseDirection true if the encoder direction should be reversed
+     */
+    public void setReverseDirection(boolean reverseDirection) {
+        // XXX: Currently uni-directional
+    	throw new UnsupportedOperationException("Simulation doesn't support reversing direction.");
+    }
+    
+    /**
+     * Set the Samples to Average which specifies the number of samples of the timer to 
+     * average when calculating the period. Perform averaging to account for 
+     * mechanical imperfections or as oversampling to increase resolution.
+     * @param samplesToAverage The number of samples to average from 1 to 127.
+     */
+    public void setSamplesToAverage(int samplesToAverage) {
+    	throw new UnsupportedOperationException("Simulation doesn't support averaging samples.");
+    }
+    
+    /**
+     * Get the Samples to Average which specifies the number of samples of the timer to 
+     * average when calculating the period. Perform averaging to account for 
+     * mechanical imperfections or as oversampling to increase resolution.
+     * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
+     */
+    public int getSamplesToAverage() {
+    	throw new UnsupportedOperationException("Simulation doesn't support averaging samples.");
+    }
+
+    /**
+     * Set which parameter of the encoder you are using as a process control variable.
+     * The encoder class supports the rate and distance parameters.
+     * @param pidSource An enum to select the parameter.
+     */
+    public void setPIDSourceParameter(PIDSourceParameter pidSource) {
+        BoundaryException.assertWithinBounds(pidSource.value, 0, 1);
+        m_pidSource = pidSource;
+    }
+
+    /**
+     * Implement the PIDSource interface.
+     *
+     * @return The current value of the selected source parameter.
+     */
+    public double pidGet() {
+        switch (m_pidSource.value) {
+        case PIDSourceParameter.kDistance_val:
+            return getDistance();
+        case PIDSourceParameter.kRate_val:
+            return getRate();
+        default:
+            return 0.0;
+        }
+    }
+
+    /*
+     * Live Window code, only does anything if live window is activated.
+     */
+    public String getSmartDashboardType(){
+        switch(m_encodingType.value)
+        {
+            case EncodingType.k4X_val:
+                return "Quadrature Encoder";
+            default:
+                return "Encoder";
+        }
+    }
+    private ITable m_table;
+    
+    /**
+     * {@inheritDoc}
+     */
+    public void initTable(ITable subtable) {
+        m_table = subtable;
+        updateTable();
+    }
+    
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable(){
+        return m_table;
+    }
+    
+    /**
+     * {@inheritDoc}
+     */
+    public void updateTable() {
+        if (m_table != null) {
+            m_table.putNumber("Speed", getRate());
+            m_table.putNumber("Distance", getDistance());
+            m_table.putNumber("Distance per Tick", m_distancePerPulse);
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public void startLiveWindowMode() {}
+    
+    /**
+     * {@inheritDoc}
+     */
+    public void stopLiveWindowMode() {}
+}

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

@@ -0,0 +1,155 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * GenericHID Interface
+ */
+public abstract class GenericHID {
+
+    /**
+     * Which hand the Human Interface Device is associated with.
+     */
+    public static class Hand {
+
+        /**
+         * The integer value representing this enumeration
+         */
+        public final int value;
+        static final int kLeft_val = 0;
+        static final int kRight_val = 1;
+        /**
+         * hand: left
+         */
+        public static final Hand kLeft = new Hand(kLeft_val);
+        /**
+         * hand: right
+         */
+        public static final Hand kRight = new Hand(kRight_val);
+
+        private Hand(int value) {
+            this.value = value;
+        }
+    }
+
+    /**
+     * Get the x position of the HID
+     * @return the x position of the HID
+     */
+    public final double getX() {
+        return getX(Hand.kRight);
+    }
+
+    /**
+     * Get the x position of HID
+     * @param hand which hand, left or right
+     * @return the x position
+     */
+    public abstract double getX(Hand hand);
+
+    /**
+     * Get the y position of the HID
+     * @return the y position
+     */
+    public final double getY() {
+        return getY(Hand.kRight);
+    }
+
+    /**
+     * Get the y position of the HID
+     * @param hand which hand, left or right
+     * @return the y position
+     */
+    public abstract double getY(Hand hand);
+
+    /**
+     * Get the z position of the HID
+     * @return the z position
+     */
+    public final double getZ() {
+        return getZ(Hand.kRight);
+    }
+
+    /**
+     * Get the z position of the HID
+     * @param hand which hand, left or right
+     * @return the z position
+     */
+    public abstract double getZ(Hand hand);
+
+    /**
+     * Get the twist value
+     * @return the twist value
+     */
+    public abstract double getTwist();
+
+    /**
+     * Get the throttle
+     * @return the throttle value
+     */
+    public abstract double getThrottle();
+
+    /**
+     * Get the raw axis
+     * @param which index of the axis
+     * @return the raw value of the selected axis
+     */
+    public abstract double getRawAxis(int which);
+
+    /**
+     * Is the trigger pressed
+     * @return true if pressed
+     */
+    public final boolean getTrigger() {
+        return getTrigger(Hand.kRight);
+    }
+
+    /**
+     * Is the trigger pressed
+     * @param hand which hand
+     * @return true if the trigger for the given hand is pressed
+     */
+    public abstract boolean getTrigger(Hand hand);
+
+    /**
+     * Is the top button pressed
+     * @return true if the top button is pressed
+     */
+    public final boolean getTop() {
+        return getTop(Hand.kRight);
+    }
+
+    /**
+     * Is the top button pressed
+     * @param hand which hand
+     * @return true if hte top button for the given hand is pressed
+     */
+    public abstract boolean getTop(Hand hand);
+
+    /**
+     * Is the bumper pressed
+     * @return true if the bumper is pressed
+     */
+    public final boolean getBumper() {
+        return getBumper(Hand.kRight);
+    }
+
+    /**
+     * Is the bumper pressed
+     * @param hand which hand
+     * @return true if hte bumper is pressed
+     */
+    public abstract boolean getBumper(Hand hand);
+
+    /**
+     * Is the given button pressed
+     * @param button which button number
+     * @return true if the button is pressed
+     */
+    public abstract boolean getRawButton(int button);
+}

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

@@ -0,0 +1,215 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimGyro;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.util.BoundaryException;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ * The Gyro class tracks the robots heading based on the starting position. As
+ * the robot rotates the new heading is computed by integrating the rate of
+ * rotation returned by the sensor. When the class is instantiated, it does a
+ * short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to
+ * determine the heading.
+ */
+public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
+
+	double m_offset;
+	int m_center;
+	boolean m_channelAllocated;
+	private PIDSourceParameter m_pidSource;
+	private SimGyro impl;
+
+	/**
+	 * Initialize the gyro. Calibrate the gyro by running for a number of
+	 * samples and computing the center value for this part. Then use the center
+	 * value as the Accumulator center value for subsequent measurements. It's
+	 * important to make sure that the robot is not moving while the centering
+	 * calculations are in progress, this is typically done when the robot is
+	 * first turned on while it's sitting at rest before the competition starts.
+	 */
+	private void initGyro(int slot, int channel) {
+		impl = new SimGyro("simulator/analog/"+slot+"/"+channel);
+		
+		reset();
+		setPIDSourceParameter(PIDSourceParameter.kAngle);
+
+		//UsageReporting.report(tResourceType.kResourceType_Gyro,
+		//  m_analog.getChannel(), m_analog.getModuleNumber() - 1);
+		LiveWindow.addSensor("Gyro", slot, channel, this);
+	}
+
+	/**
+	 * Gyro constructor given a slot and a channel. .
+	 * 
+	 * @param slot
+	 *            The cRIO slot for the analog module the gyro is connected to.
+	 * @param channel
+	 *            The analog channel the gyro is connected to.
+	 */
+	public Gyro(int slot, int channel) {
+		m_channelAllocated = true;
+		initGyro(slot, channel);
+	}
+
+	/**
+	 * Gyro constructor with only a channel.
+	 * 
+	 * Use the default analog module slot.
+	 * 
+	 * @param channel
+	 *            The analog channel the gyro is connected to.
+	 */
+	public Gyro(int channel) {
+		m_channelAllocated = true;
+		initGyro(1, channel);
+	}
+
+	/**
+	 * Gyro constructor with a precreated analog channel object. Use this
+	 * constructor when the analog channel needs to be shared. There is no
+	 * reference counting when an AnalogChannel is passed to the gyro.
+	 * 
+	 * @param channel
+	 *            The AnalogChannel object that the gyro is connected to.
+	 */
+	// Not Supported: public Gyro(AnalogChannel channel) {
+
+	/**
+	 * Reset the gyro. Resets the gyro to a heading of zero. This can be used if
+	 * there is significant drift in the gyro and it needs to be recalibrated
+	 * after it has been running.
+	 */
+	public void reset() {
+		impl.reset();
+	}
+
+	/**
+	 * Delete (free) the accumulator and the analog components used for the
+	 * gyro.
+	 */
+	public void free() {
+	}
+
+	/**
+	 * Return the actual angle in degrees that the robot is currently facing.
+	 * 
+	 * The angle is based on the current accumulator value corrected by the
+	 * oversampling rate, the gyro type and the A/D calibration values. The
+	 * angle is continuous, that is can go beyond 360 degrees. This make
+	 * algorithms that wouldn't want to see a discontinuity in the gyro output
+	 * as it sweeps past 0 on the second time around.
+	 * 
+	 * @return the current heading of the robot in degrees. This heading is
+	 *         based on integration of the returned rate from the gyro.
+	 */
+	public double getAngle() {
+		return impl.getAngle();
+	}
+
+	/**
+	 * Return the rate of rotation of the gyro
+	 * 
+	 * The rate is based on the most recent reading of the gyro analog value
+	 * 
+	 * @return the current rate in degrees per second
+	 */
+	public double getRate() {
+		return impl.getVelocity();
+	}
+
+	/**
+	 * Set the gyro type based on the sensitivity. This takes the number of
+	 * volts/degree/second sensitivity of the gyro and uses it in subsequent
+	 * calculations to allow the code to work with multiple gyros.
+	 * 
+	 * @param voltsPerDegreePerSecond
+	 *            The type of gyro specified as the voltage that represents one
+	 *            degree/second.
+	 */
+	public void setSensitivity(double voltsPerDegreePerSecond) {
+		throw new UnsupportedOperationException("Simulation doesn't currently support setting the sensitivity.");
+	}
+
+	/**
+	 * Set which parameter of the encoder you are using as a process control
+	 * variable. The Gyro class supports the rate and angle parameters
+	 * 
+	 * @param pidSource
+	 *            An enum to select the parameter.
+	 */
+	public void setPIDSourceParameter(PIDSourceParameter pidSource) {
+		BoundaryException.assertWithinBounds(pidSource.value, 1, 2);
+		m_pidSource = pidSource;
+	}
+
+	/**
+	 * Get the angle of the gyro for use with PIDControllers
+	 * 
+	 * @return the current angle according to the gyro
+	 */
+	public double pidGet() {
+		switch (m_pidSource.value) {
+		case PIDSourceParameter.kRate_val:
+			return getRate();
+		case PIDSourceParameter.kAngle_val:
+			return getAngle();
+		default:
+			return 0.0;
+		}
+	}
+
+	/*
+	 * Live Window code, only does anything if live window is activated.
+	 */
+	public String getSmartDashboardType() {
+		return "Gyro";
+	}
+
+	private ITable m_table;
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void initTable(ITable subtable) {
+		m_table = subtable;
+		updateTable();
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public ITable getTable() {
+		return m_table;
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void updateTable() {
+		if (m_table != null) {
+			m_table.putNumber("Value", getAngle());
+		}
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void startLiveWindowMode() {
+	}
+
+	/**
+	 * {@inheritDoc}
+	 */
+	public void stopLiveWindowMode() {
+	}
+}

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

@@ -0,0 +1,281 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * IterativeRobot implements a specific type of Robot Program framework, extending the RobotBase class.
+ *
+ * The IterativeRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * This class is intended to implement the "old style" default code, by providing
+ * the following functions which are called by the main loop, startCompetition(), at the appropriate times:
+ *
+ * robotInit() -- provide for initialization at robot power-on
+ *
+ * init() functions -- each of the following functions is called once when the
+ *                     appropriate mode is entered:
+ *  - DisabledInit()   -- called only when first disabled
+ *  - AutonomousInit() -- called each and every time autonomous is entered from another mode
+ *  - TeleopInit()     -- called each and every time teleop is entered from another mode
+ *  - TestInit()       -- called each and every time test mode is entered from anothermode
+ *
+ * Periodic() functions -- each of these functions is called iteratively at the
+ *                         appropriate periodic rate (aka the "slow loop").  The period of
+ *                         the iterative robot is synced to the driver station control packets,
+ *                         giving a periodic frequency of about 50Hz (50 times per second).
+ *   - disabledPeriodic()
+ *   - autonomousPeriodic()
+ *   - teleopPeriodic()
+ *   - testPeriodoc()
+ *
+ */
+public class IterativeRobot extends RobotBase {
+    private boolean m_disabledInitialized;
+    private boolean m_autonomousInitialized;
+    private boolean m_teleopInitialized;
+    private boolean m_testInitialized;
+
+    /**
+     * Constructor for RobotIterativeBase
+     *
+     * The constructor initializes the instance variables for the robot to indicate
+     * the status of initialization for disabled, autonomous, and teleop code.
+     */
+    public IterativeRobot() {
+        // set status for initialization of disabled, autonomous, and teleop code.
+        m_disabledInitialized = false;
+        m_autonomousInitialized = false;
+        m_teleopInitialized = false;
+        m_testInitialized = false;
+    }
+
+    /**
+     * Provide an alternate "main loop" via startCompetition().
+     *
+     */
+    public void startCompetition() {
+        // UsageReporting.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
+
+        robotInit();
+
+        // tracing support:
+        final int TRACE_LOOP_MAX = 100;
+        int loopCount = TRACE_LOOP_MAX;
+        Object marker = null;
+        boolean didDisabledPeriodic = false;
+        boolean didAutonomousPeriodic = false;
+        boolean didTeleopPeriodic = false;
+        boolean didTestPeriodic = false;
+
+        // loop forever, calling the appropriate mode-dependent function
+        LiveWindow.setEnabled(false);
+        while (true) {
+            // Call the appropriate function depending upon the current robot mode
+            if (isDisabled()) {
+                // call DisabledInit() if we are now just entering disabled mode from
+                // either a different mode or from power-on
+                if (!m_disabledInitialized) {
+                    LiveWindow.setEnabled(false);
+                    disabledInit();
+                    m_disabledInitialized = true;
+                    // reset the initialization flags for the other modes
+                    m_autonomousInitialized = false;
+                    m_teleopInitialized = false;
+                    m_testInitialized = false;
+                }
+                if (nextPeriodReady()) {
+                	// TODO: FRC_NetworkCommunicationsLibrary.FRC_NetworkCommunication_observeUserProgramDisabled();
+                    disabledPeriodic();
+                    didDisabledPeriodic = true;
+                }
+            } else if (isTest()) {
+                // call TestInit() if we are now just entering test mode from either
+                // a different mode or from power-on
+                if (!m_testInitialized) {
+                    LiveWindow.setEnabled(true);
+                    testInit();
+                    m_testInitialized = true;
+                    m_autonomousInitialized = false;
+                    m_teleopInitialized = false;
+                    m_disabledInitialized = false;
+                }
+                if (nextPeriodReady()) {
+                	// TODO: FRC_NetworkCommunicationsLibrary.FRC_NetworkCommunication_observeUserProgramTest();
+                    testPeriodic();
+                    didTestPeriodic = true;
+                }
+            } else if (isAutonomous()) {
+                // call Autonomous_Init() if this is the first time
+                // we've entered autonomous_mode
+                if (!m_autonomousInitialized) {
+                    LiveWindow.setEnabled(false);
+                    // KBS NOTE: old code reset all PWMs and relays to "safe values"
+                    // whenever entering autonomous mode, before calling
+                    // "Autonomous_Init()"
+                    autonomousInit();
+                    m_autonomousInitialized = true;
+                    m_testInitialized = false;
+                    m_teleopInitialized = false;
+                    m_disabledInitialized = false;
+                }
+                if (nextPeriodReady()) {
+                    // TODO: getWatchdog().feed();
+                    // TODO: FRC_NetworkCommunicationsLibrary.FRC_NetworkCommunication_observeUserProgramAutonomous();
+                    autonomousPeriodic();
+                    didAutonomousPeriodic = true;
+                }
+            } else {
+                // call Teleop_Init() if this is the first time
+                // we've entered teleop_mode
+                if (!m_teleopInitialized) {
+                    LiveWindow.setEnabled(false);
+                    teleopInit();
+                    m_teleopInitialized = true;
+                    m_testInitialized = false;
+                    m_autonomousInitialized = false;
+                    m_disabledInitialized = false;
+                }
+                if (nextPeriodReady()) {
+                    // TODO: getWatchdog().feed();
+                    // TODO: FRC_NetworkCommunicationsLibrary.FRC_NetworkCommunication_observeUserProgramTeleop();
+                    teleopPeriodic();
+                    didTeleopPeriodic = true;
+                }
+            }
+        }
+    }
+
+    /**
+     * Determine if the appropriate next periodic function should be called.
+     * 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);
+        } catch (InterruptedException ex) {}
+    	return true;
+    }
+
+    /* ----------- Overridable initialization code -----------------*/
+
+    /**
+     * Robot-wide initialization code should go here.
+     *
+     * Users should override this method for default Robot-wide initialization which will
+     * be called when the robot is first powered on.  It will be called exactly 1 time.
+     */
+    public void robotInit() {
+        System.out.println("Default IterativeRobot.robotInit() method... Overload me!");
+    }
+
+    /**
+     * Initialization code for disabled mode should go here.
+     *
+     * Users should override this method for initialization code which will be called each time
+     * the robot enters disabled mode.
+     */
+    public void disabledInit() {
+        System.out.println("Default IterativeRobot.disabledInit() method... Overload me!");
+    }
+
+    /**
+     * Initialization code for autonomous mode should go here.
+     *
+     * Users should override this method for initialization code which will be called each time
+     * the robot enters autonomous mode.
+     */
+    public void autonomousInit() {
+        System.out.println("Default IterativeRobot.autonomousInit() method... Overload me!");
+    }
+
+    /**
+     * Initialization code for teleop mode should go here.
+     *
+     * Users should override this method for initialization code which will be called each time
+     * the robot enters teleop mode.
+     */
+    public void teleopInit() {
+        System.out.println("Default IterativeRobot.teleopInit() method... Overload me!");
+    }
+
+    /**
+     * Initialization code for test mode should go here.
+     *
+     * Users should override this method for initialization code which will be called each time
+     * the robot enters test mode.
+     */
+    public void testInit() {
+        System.out.println("Default IterativeRobot.testInit() method... Overload me!");
+    }
+
+    /* ----------- Overridable periodic code -----------------*/
+
+    private boolean dpFirstRun = true;
+    /**
+     * Periodic code for disabled mode should go here.
+     *
+     * Users should override this method for code which will be called periodically at a regular
+     * rate while the robot is in disabled mode.
+     */
+    public void disabledPeriodic() {
+        if (dpFirstRun) {
+            System.out.println("Default IterativeRobot.disabledPeriodic() method... Overload me!");
+            dpFirstRun = false;
+        }
+        Timer.delay(0.001);
+    }
+
+    private boolean apFirstRun = true;
+
+    /**
+     * Periodic code for autonomous mode should go here.
+     *
+     * Users should override this method for code which will be called periodically at a regular
+     * rate while the robot is in autonomous mode.
+     */
+    public void autonomousPeriodic() {
+        if (apFirstRun) {
+            System.out.println("Default IterativeRobot.autonomousPeriodic() method... Overload me!");
+            apFirstRun = false;
+        }
+        Timer.delay(0.001);
+    }
+
+    private boolean tpFirstRun = true;
+
+    /**
+     * Periodic code for teleop mode should go here.
+     *
+     * Users should override this method for code which will be called periodically at a regular
+     * rate while the robot is in teleop mode.
+     */
+    public void teleopPeriodic() {
+        if (tpFirstRun) {
+            System.out.println("Default IterativeRobot.teleopPeriodic() method... Overload me!");
+            tpFirstRun = false;
+        }
+        Timer.delay(0.001);
+    }
+
+    private boolean tmpFirstRun = true;
+
+    /**
+     * Periodic code for test mode should go here
+     *
+     * Users should override this method for code which will be called periodically at a regular rate
+     * while the robot is in test mode.
+     */
+    public void testPeriodic() {
+        if (tmpFirstRun) {
+            System.out.println("Default IterativeRobot.testPeriodic() method... Overload me!");
+            tmpFirstRun = false;
+        }
+    }
+}

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

@@ -0,0 +1,209 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimSpeedController;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+
+/**
+ * CTRE Talon Speed Controller
+ */
+public class Jaguar implements SpeedController, PIDOutput, MotorSafety, LiveWindowSendable {
+	private int module, channel;
+	private SimSpeedController impl;
+
+    /**
+     * Common initialization code called by all constructors.
+     */
+    private void initTalon(final int slot, final int channel) {
+    	this.module = slot;
+    	this.channel = channel;
+    	impl = new SimSpeedController("simulator/pwm/"+slot+"/"+channel);
+    	
+        m_safetyHelper = new MotorSafetyHelper(this);
+        m_safetyHelper.setExpiration(0.0);
+        m_safetyHelper.setSafetyEnabled(false);
+        
+        // LiveWindow.addActuator("Talon", getModuleNumber(), getChannel(), this);
+        // UsageReporting.report(tResourceType.kResourceType_Talon, getChannel(), getModuleNumber()-1);
+    }
+
+    /**
+     * Constructor that assumes the default digital module.
+     *
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Jaguar(final int channel) {
+        initTalon(1, channel);
+    }
+
+    /**
+     * Constructor that specifies the digital module.
+     *
+     * @param slot The slot in the chassis that the digital module is plugged into.
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Jaguar(final int slot, final int channel) {
+        initTalon(slot, channel);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * @deprecated For compatibility with CANJaguar
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
+     */
+    public void set(double speed, byte syncGroup) {
+    	impl.set(speed, syncGroup);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed value between -1.0 and 1.0 to set.
+     */
+    public void set(double speed) {
+    	impl.set(speed);
+    }
+
+    /**
+     * Get the recently set value of the PWM.
+     *
+     * @return The most recently set value for the PWM between -1.0 and 1.0.
+     */
+    public double get() {
+        return impl.get();
+    }
+	
+    /**
+     * Disable the speed controller
+     */
+    public void disable() {
+		impl.set(0);
+	}
+
+    /**
+     * Write out the PID value as seen in the PIDOutput base object.
+     *
+     * @param output Write out the PWM value as was found in the PIDController
+     */
+    public void pidWrite(double output) {
+        impl.pidWrite(output);
+    }
+
+    //// MotorSafety Methods
+    private MotorSafetyHelper m_safetyHelper;
+    
+	@Override
+	public void setExpiration(double timeout) {
+        m_safetyHelper.setExpiration(timeout);
+	}
+
+	@Override
+	public double getExpiration() {
+        return m_safetyHelper.getExpiration();
+	}
+
+	@Override
+	public boolean isAlive() {
+        return m_safetyHelper.isAlive();
+	}
+
+	@Override
+	public void stopMotor() {
+        disable();
+	}
+
+	@Override
+	public void setSafetyEnabled(boolean enabled) {
+        m_safetyHelper.setSafetyEnabled(enabled);
+	}
+
+	@Override
+	public boolean isSafetyEnabled() {
+        return m_safetyHelper.isSafetyEnabled();
+	}
+
+	@Override
+	public String getDescription() {
+        return "PWM "+channel+" on module "+module;
+	}
+
+	//// LiveWindow Methods
+    private ITable m_table;
+    private ITableListener m_table_listener;
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void initTable(ITable subtable) {
+        m_table = subtable;
+        updateTable();
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public ITable getTable() {
+        return m_table;
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public String getSmartDashboardType() {
+        return "Speed Controller";
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void updateTable() {
+        if (m_table != null) {
+            m_table.putNumber("Value", get());
+        }
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void startLiveWindowMode() {
+        set(0.0); // Stop for safety
+        m_table_listener = new ITableListener() {
+            public void valueChanged(ITable itable, String key, Object value, boolean bln) {
+                set(((Double) value).doubleValue());
+            }
+        };
+        m_table.addTableListener("Value", m_table_listener, true);
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void stopLiveWindowMode() {
+        set(0); // Stop for safety
+        // TODO: Broken, should only remove the listener from "Value" only.
+        m_table.removeTableListener(m_table_listener);
+	}
+}

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

@@ -0,0 +1,350 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ * Handle input from standard Joysticks connected to the Driver Station.
+ * This class handles standard input that comes from the Driver Station. Each time a value is requested
+ * the most recent value is returned. There is a single class instance for each joystick and the mapping
+ * of ports to hardware buttons depends on the code in the driver station.
+ */
+public class Joystick extends GenericHID {
+
+    static final byte kDefaultXAxis = 1;
+    static final byte kDefaultYAxis = 2;
+    static final byte kDefaultZAxis = 3;
+    static final byte kDefaultTwistAxis = 3;
+    static final byte kDefaultThrottleAxis = 4;
+    static final int kDefaultTriggerButton = 1;
+    static final int kDefaultTopButton = 2;
+
+    /**
+     * Represents an analog axis on a joystick.
+     */
+    public static class AxisType {
+
+        /**
+         * The integer value representing this enumeration
+         */
+        public final int value;
+        static final int kX_val = 0;
+        static final int kY_val = 1;
+        static final int kZ_val = 2;
+        static final int kTwist_val = 3;
+        static final int kThrottle_val = 4;
+        static final int kNumAxis_val = 5;
+        /**
+         * axis: x-axis
+         */
+        public static final AxisType kX = new AxisType(kX_val);
+        /**
+         * axis: y-axis
+         */
+        public static final AxisType kY = new AxisType(kY_val);
+        /**
+         * axis: z-axis
+         */
+        public static final AxisType kZ = new AxisType(kZ_val);
+        /**
+         * axis: twist
+         */
+        public static final AxisType kTwist = new AxisType(kTwist_val);
+        /**
+         * axis: throttle
+         */
+        public static final AxisType kThrottle = new AxisType(kThrottle_val);
+        /**
+         * axis: number of axis
+         */
+        public static final AxisType kNumAxis = new AxisType(kNumAxis_val);
+
+        private AxisType(int value) {
+            this.value = value;
+        }
+    }
+
+    /**
+     * Represents a digital button on the JoyStick
+     */
+    public static class ButtonType {
+
+        /**
+         * The integer value representing this enumeration
+         */
+        public final int value;
+        static final int kTrigger_val = 0;
+        static final int kTop_val = 1;
+        static final int kNumButton_val = 2;
+        /**
+         * button: trigger
+         */
+        public static final ButtonType kTrigger = new ButtonType((kTrigger_val));
+        /**
+         * button: top button
+         */
+        public static final ButtonType kTop = new ButtonType(kTop_val);
+        /**
+         * button: num button types
+         */
+        public static final ButtonType kNumButton = new ButtonType((kNumButton_val));
+
+        private ButtonType(int value) {
+            this.value = value;
+        }
+    }
+    private final int m_port;
+    private final byte[] m_axes;
+    private final byte[] m_buttons;
+
+    /**
+     * Construct an instance of a joystick.
+     * The joystick index is the usb port on the drivers station.
+     *
+     * @param port The port on the driver station that the joystick is plugged into.
+     */
+    public Joystick(final int port) {
+        this(port, AxisType.kNumAxis.value, ButtonType.kNumButton.value);
+
+        m_axes[AxisType.kX.value] = kDefaultXAxis;
+        m_axes[AxisType.kY.value] = kDefaultYAxis;
+        m_axes[AxisType.kZ.value] = kDefaultZAxis;
+        m_axes[AxisType.kTwist.value] = kDefaultTwistAxis;
+        m_axes[AxisType.kThrottle.value] = kDefaultThrottleAxis;
+
+        m_buttons[ButtonType.kTrigger.value] = kDefaultTriggerButton;
+        m_buttons[ButtonType.kTop.value] = kDefaultTopButton;
+
+        //UsageReporting.report(tResourceType.kResourceType_Joystick, port);
+    }
+
+    /**
+     * Protected version of the constructor to be called by sub-classes.
+     *
+     * This constructor allows the subclass to configure the number of constants
+     * for axes and buttons.
+     *
+     * @param port The port on the driver station that the joystick is plugged into.
+     * @param numAxisTypes The number of axis types in the enum.
+     * @param numButtonTypes The number of button types in the enum.
+     */
+    protected Joystick(int port, int numAxisTypes, int numButtonTypes) {
+        m_axes = new byte[numAxisTypes];
+        m_buttons = new byte[numButtonTypes];
+        m_port = port;
+    }
+
+    /**
+     * Get the X value of the joystick.
+     * This depends on the mapping of the joystick connected to the current port.
+     *
+     * @param hand Unused
+     * @return The X value of the joystick.
+     */
+    public double getX(Hand hand) {
+        return getRawAxis(m_axes[AxisType.kX.value]);
+    }
+
+    /**
+     * Get the Y value of the joystick.
+     * This depends on the mapping of the joystick connected to the current port.
+     *
+     * @param hand Unused
+     * @return The Y value of the joystick.
+     */
+    public double getY(Hand hand) {
+        return getRawAxis(m_axes[AxisType.kY.value]);
+    }
+
+    /**
+     * Get the Z value of the joystick.
+     * This depends on the mapping of the joystick connected to the current port.
+     *
+     * @param hand Unused
+     * @return The Z value of the joystick.
+     */
+    public double getZ(Hand hand) {
+        return getRawAxis(m_axes[AxisType.kZ.value]);
+    }
+
+    /**
+     * Get the twist value of the current joystick.
+     * This depends on the mapping of the joystick connected to the current port.
+     *
+     * @return The Twist value of the joystick.
+     */
+    public double getTwist() {
+        return getRawAxis(m_axes[AxisType.kTwist.value]);
+    }
+
+    /**
+     * Get the throttle value of the current joystick.
+     * This depends on the mapping of the joystick connected to the current port.
+     *
+     * @return The Throttle value of the joystick.
+     */
+    public double getThrottle() {
+        return getRawAxis(m_axes[AxisType.kThrottle.value]);
+    }
+
+    /**
+     * Get the value of the axis.
+     *
+     * @param axis The axis to read [1-6].
+     * @return The value of the axis.
+     */
+    public double getRawAxis(final int axis) {
+        return DriverStation.getInstance().getStickAxis(m_port, axis);
+    }
+
+    /**
+     * For the current joystick, return the axis determined by the argument.
+     *
+     * This is for cases where the joystick axis is returned programatically, otherwise one of the
+     * previous functions would be preferable (for example getX()).
+     *
+     * @param axis The axis to read.
+     * @return The value of the axis.
+     */
+    public double getAxis(final AxisType axis) {
+        switch (axis.value) {
+        case AxisType.kX_val:
+            return getX();
+        case AxisType.kY_val:
+            return getY();
+        case AxisType.kZ_val:
+            return getZ();
+        case AxisType.kTwist_val:
+            return getTwist();
+        case AxisType.kThrottle_val:
+            return getThrottle();
+        default:
+            return 0.0;
+        }
+    }
+
+    /**
+     * Read the state of the trigger on the joystick.
+     *
+     * Look up which button has been assigned to the trigger and read its state.
+     *
+     * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
+     * @return The state of the trigger.
+     */
+    public boolean getTrigger(Hand hand) {
+        return getRawButton(m_buttons[ButtonType.kTrigger.value]);
+    }
+
+    /**
+     * Read the state of the top button on the joystick.
+     *
+     * Look up which button has been assigned to the top and read its state.
+     *
+     * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
+     * @return The state of the top button.
+     */
+    public boolean getTop(Hand hand) {
+        return getRawButton(m_buttons[ButtonType.kTop.value]);
+    }
+
+    /**
+     * This is not supported for the Joystick.
+     * This method is only here to complete the GenericHID interface.
+     *
+     * @param hand This parameter is ignored for the Joystick class and is only here to complete the GenericHID interface.
+     * @return The state of the bumper (always false)
+     */
+    public boolean getBumper(Hand hand) {
+        return false;
+    }
+
+    /**
+     * Get the button value for buttons 1 through 12.
+     *
+     * The buttons are returned in a single 16 bit value with one bit representing the state
+     * of each button. The appropriate button is returned as a boolean value.
+     *
+     * @param button The button number to be read.
+     * @return The state of the button.
+     */
+    public boolean getRawButton(final int button) {
+        return DriverStation.getInstance().getStickButton(m_port, button);
+    }
+
+    /**
+     * Get buttons based on an enumerated type.
+     *
+     * The button type will be looked up in the list of buttons and then read.
+     *
+     * @param button The type of button to read.
+     * @return The state of the button.
+     */
+    public boolean getButton(ButtonType button) {
+        switch (button.value) {
+        case ButtonType.kTrigger_val:
+            return getTrigger();
+        case ButtonType.kTop_val:
+            return getTop();
+        default:
+            return false;
+        }
+    }
+
+    /**
+     * Get the magnitude of the direction vector formed by the joystick's
+     * current position relative to its origin
+     *
+     * @return The magnitude of the direction vector
+     */
+    public double getMagnitude() {
+        return Math.sqrt(Math.pow(getX(), 2) + Math.pow(getY(), 2));
+    }
+
+    /**
+     * Get the direction of the vector formed by the joystick and its origin
+     * in radians
+     *
+     * @return The direction of the vector in radians
+     */
+    public double getDirectionRadians() {
+        return Math.atan2(getX(), -getY());
+    }
+
+    /**
+     * Get the direction of the vector formed by the joystick and its origin
+     * in degrees
+     *
+     * uses acos(-1) to represent Pi due to absence of readily accessable Pi
+     * constant in C++
+     *
+     * @return The direction of the vector in degrees
+     */
+    public double getDirectionDegrees() {
+        return Math.toDegrees(getDirectionRadians());
+    }
+
+    /**
+     * Get the channel currently associated with the specified axis.
+     *
+     * @param axis The axis to look up the channel for.
+     * @return The channel fr the axis.
+     */
+    public int getAxisChannel(AxisType axis) {
+        return m_axes[axis.value];
+    }
+
+    /**
+     * Set the channel associated with a specified axis.
+     *
+     * @param axis The axis to set the channel for.
+     * @param channel The channel to set the axis to.
+     */
+    public void setAxisChannel(AxisType axis, int channel) {
+        m_axes[axis.value] = (byte) channel;
+    }
+}

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

@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ *
+ * @author brad
+ */
+public interface MotorSafety {
+    public static final double DEFAULT_SAFETY_EXPIRATION = 0.1;
+    void setExpiration(double timeout);
+    double getExpiration();
+    boolean isAlive();
+    void stopMotor();
+    void setSafetyEnabled(boolean enabled);
+    boolean isSafetyEnabled();
+    String getDescription();
+}

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

@@ -0,0 +1,124 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * The MotorSafetyHelper object is constructed for every object that wants to implement the Motor
+ * Safety protocol. The helper object has the code to actually do the timing and call the
+ * motors Stop() method when the timeout expires. The motor object is expected to call the
+ * Feed() method whenever the motors value is updated.
+ *
+ * @author brad
+ */
+public class MotorSafetyHelper {
+
+    double m_expiration;
+    boolean m_enabled;
+    double m_stopTime;
+    MotorSafety m_safeObject;
+    MotorSafetyHelper m_nextHelper;
+    static MotorSafetyHelper m_headHelper = null;
+
+    /**
+     * The constructor for a MotorSafetyHelper object.
+     * The helper object is constructed for every object that wants to implement the Motor
+     * Safety protocol. The helper object has the code to actually do the timing and call the
+     * motors Stop() method when the timeout expires. The motor object is expected to call the
+     * Feed() method whenever the motors value is updated.
+     *
+     * @param safeObject a pointer to the motor object implementing MotorSafety. This is used
+     * to call the Stop() method on the motor.
+     */
+    public MotorSafetyHelper(MotorSafety safeObject) {
+        m_safeObject = safeObject;
+        m_enabled = false;
+        m_expiration = MotorSafety.DEFAULT_SAFETY_EXPIRATION;
+        m_stopTime = Timer.getFPGATimestamp();
+        m_nextHelper = m_headHelper;
+        m_headHelper = this;
+    }
+
+    /**
+     * Feed the motor safety object.
+     * Resets the timer on this object that is used to do the timeouts.
+     */
+    public void feed() {
+        m_stopTime = Timer.getFPGATimestamp() + m_expiration;
+    }
+
+    /**
+     * Set the expiration time for the corresponding motor safety object.
+     * @param expirationTime The timeout value in seconds.
+     */
+    public void setExpiration(double expirationTime) {
+        m_expiration = expirationTime;
+    }
+
+    /**
+     * Retrieve the timeout value for the corresponding motor safety object.
+     * @return the timeout value in seconds.
+     */
+    public double getExpiration() {
+        return m_expiration;
+    }
+
+    /**
+     * Determine of the motor is still operating or has timed out.
+     * @return a true value if the motor is still operating normally and hasn't timed out.
+     */
+    public boolean isAlive() {
+        return !m_enabled || m_stopTime > Timer.getFPGATimestamp();
+    }
+
+    /**
+     * Check if this motor has exceeded its timeout.
+     * This method is called periodically to determine if this motor has exceeded its timeout
+     * value. If it has, the stop method is called, and the motor is shut down until its value is
+     * updated again.
+     */
+    public void check() {
+        // TODO: DriverStation ds = DriverStation.getInstance();
+        // TODO: if (!m_enabled || ds.isDisabled() || ds.isTest())
+        // TODO:    return;
+        if (m_stopTime < Timer.getFPGATimestamp()) {
+            System.err.println(m_safeObject.getDescription() + "... Output not updated often enough.");
+
+            m_safeObject.stopMotor();
+        }
+    }
+
+    /**
+     * Enable/disable motor safety for this device
+     * Turn on and off the motor safety option for this PWM object.
+     * @param enabled True if motor safety is enforced for this object
+     */
+    public void setSafetyEnabled(boolean enabled) {
+        m_enabled = enabled;
+    }
+
+    /**
+     * Return the state of the motor safety enabled flag
+     * Return if the motor safety is currently enabled for this devicce.
+     * @return True if motor safety is enforced for this device
+     */
+    public boolean isSafetyEnabled() {
+        return m_enabled;
+    }
+
+    /**
+     * Check the motors to see if any have timed out.
+     * This static  method is called periodically to poll all the motors and stop any that have
+     * timed out.
+     */
+    //TODO: these should be synchronized with the setting methods in case it's called from a different thread
+    public static void checkMotors() {
+        for (MotorSafetyHelper msh = m_headHelper; msh != null; msh = msh.m_nextHelper) {
+            msh.check();
+        }
+    }
+}

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

@@ -0,0 +1,14 @@
+package edu.wpi.first.wpilibj;
+
+
+/**
+ * The interface for sendable objects that gives the sendable a default name in the Smart Dashboard
+ *
+ */
+public interface NamedSendable extends Sendable {
+
+    /**
+     * @return the name of the subtable of SmartDashboard that the Sendable object will use
+     */
+    public String getName();
+}

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

@@ -0,0 +1,588 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+import edu.wpi.first.wpilibj.util.BoundaryException;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes
+ * care of the integral calculations, as well as writing the given
+ * PIDOutput
+ */
+public class PIDController implements LiveWindowSendable, Controller {
+
+    public static final double kDefaultPeriod = .05;
+    private static int instances = 0;
+    private double m_P;			// factor for "proportional" control
+    private double m_I;			// factor for "integral" control
+    private double m_D;			// factor for "derivative" control
+    private double m_F;                 // factor for feedforward term
+    private double m_maximumOutput = 1.0;	// |maximum output|
+    private double m_minimumOutput = -1.0;	// |minimum output|
+    private double m_maximumInput = 0.0;		// maximum input - limit setpoint to this
+    private double m_minimumInput = 0.0;		// minimum input - limit setpoint to this
+    private boolean m_continuous = false;	// do the endpoints wrap around? eg. Absolute encoder
+    private boolean m_enabled = false; 			//is the pid controller enabled
+    private double m_prevError = 0.0;	// the prior sensor input (used to compute velocity)
+    private double m_totalError = 0.0; //the sum of the errors for use in the integral calc
+    private Tolerance m_tolerance;	//the tolerance object used to check if on target
+    private double m_setpoint = 0.0;
+    private double m_error = 0.0;
+    private double m_result = 0.0;
+    private double m_period = kDefaultPeriod;
+    PIDSource m_pidInput;
+    PIDOutput m_pidOutput;
+    java.util.Timer m_controlLoop;
+    private boolean m_usingPercentTolerance;
+
+    /**
+     * Tolerance is the type of tolerance used to specify if the PID controller is on target.
+     * The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
+     * specify types of tolerance specifications to use.
+     */
+    public interface Tolerance {
+        public boolean onTarget();
+    }
+
+    public class PercentageTolerance implements Tolerance {
+        double percentage;
+
+        PercentageTolerance(double value) {
+            percentage = value;
+        }
+
+        public boolean onTarget() {
+            return (Math.abs(getError()) < percentage / 100
+                    * (m_maximumInput - m_minimumInput));
+        }
+    }
+
+    public class AbsoluteTolerance implements Tolerance {
+        double value;
+
+        AbsoluteTolerance(double value) {
+            this.value = value;
+        }
+
+        public boolean onTarget() {
+            return Math.abs(getError()) < value;
+        }
+    }
+
+    public class NullTolerance implements Tolerance {
+
+        public boolean onTarget() {
+            throw new RuntimeException("No tolerance value set when using PIDController.onTarget()");
+        }
+    }
+
+    private class PIDTask extends TimerTask {
+
+        private PIDController m_controller;
+
+        public PIDTask(PIDController controller) {
+            if (controller == null) {
+                throw new NullPointerException("Given PIDController was null");
+            }
+            m_controller = controller;
+        }
+
+        public void run() {
+            m_controller.calculate();
+        }
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D, and F
+     * @param Kp the proportional coefficient
+     * @param Ki the integral coefficient
+     * @param Kd the derivative coefficient
+     * @param Kf the feed forward term
+     * @param source The PIDSource object that is used to get values
+     * @param output The PIDOutput object that is set to the output percentage
+     * @param period the loop time for doing calculations. This particularly effects calculations of the
+     * integral and differential terms. The default is 50ms.
+     */
+    public PIDController(double Kp, double Ki, double Kd, double Kf,
+                         PIDSource source, PIDOutput output,
+                         double period) {
+
+        if (source == null) {
+            throw new NullPointerException("Null PIDSource was given");
+        }
+        if (output == null) {
+            throw new NullPointerException("Null PIDOutput was given");
+        }
+
+        m_controlLoop = new java.util.Timer();
+
+
+        m_P = Kp;
+        m_I = Ki;
+        m_D = Kd;
+        m_F = Kf;
+
+        m_pidInput = source;
+        m_pidOutput = output;
+        m_period = period;
+
+        m_controlLoop.schedule(new PIDTask(this), 0L, (long) (m_period * 1000));
+
+        instances++;
+        // UsageReporting.report(tResourceType.kResourceType_PIDController, instances);
+        m_tolerance = new NullTolerance();
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D and period
+     * @param Kp
+     * @param Ki
+     * @param Kd
+     * @param source
+     * @param output
+     * @param period
+     */
+    public PIDController(double Kp, double Ki, double Kd,
+                         PIDSource source, PIDOutput output,
+                         double period) {
+        this(Kp, Ki, Kd, 0.0, source, output, period);
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+     * @param Kp the proportional coefficient
+     * @param Ki the integral coefficient
+     * @param Kd the derivative coefficient
+     * @param source The PIDSource object that is used to get values
+     * @param output The PIDOutput object that is set to the output percentage
+     */
+    public PIDController(double Kp, double Ki, double Kd,
+                         PIDSource source, PIDOutput output) {
+        this(Kp, Ki, Kd, source, output, kDefaultPeriod);
+    }
+
+    /**
+     * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+     * @param Kp the proportional coefficient
+     * @param Ki the integral coefficient
+     * @param Kd the derivative coefficient
+     * @param source The PIDSource object that is used to get values
+     * @param output The PIDOutput object that is set to the output percentage
+     */
+    public PIDController(double Kp, double Ki, double Kd, double Kf,
+                         PIDSource source, PIDOutput output) {
+        this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
+    }
+
+    /**
+     * Free the PID object
+     */
+    public void free() {
+        m_controlLoop.cancel();
+        m_controlLoop = null;
+    }
+
+    /**
+     * Read the input, calculate the output accordingly, and write to the output.
+     * This should only be called by the PIDTask
+     * and is created during initialization.
+     */
+    private void calculate() {
+        boolean enabled;
+        PIDSource pidInput;
+
+        synchronized (this) {
+            if (m_pidInput == null) {
+                return;
+            }
+            if (m_pidOutput == null) {
+                return;
+            }
+            enabled = m_enabled; // take snapshot of these values...
+            pidInput = m_pidInput;
+        }
+
+        if (enabled) {
+            double input = pidInput.pidGet();
+            double result;
+            PIDOutput pidOutput = null;
+
+            synchronized (this) {
+                m_error = m_setpoint - input;
+                if (m_continuous) {
+                    if (Math.abs(m_error)
+                            > (m_maximumInput - m_minimumInput) / 2) {
+                        if (m_error > 0) {
+                            m_error = m_error - m_maximumInput + m_minimumInput;
+                        } else {
+                            m_error = m_error
+                                      + m_maximumInput - m_minimumInput;
+                        }
+                    }
+                }
+
+                if (m_I != 0) {
+                    double potentialIGain = (m_totalError + m_error) * m_I;
+                    if (potentialIGain < m_maximumOutput) {
+                        if (potentialIGain > m_minimumOutput) {
+                            m_totalError += m_error;
+                        } else {
+                            m_totalError = m_minimumOutput / m_I;
+                        }
+                    } else {
+                        m_totalError = m_maximumOutput / m_I;
+                    }
+                }
+
+                m_result = m_P * m_error + m_I * m_totalError + m_D * (m_error - m_prevError) + m_setpoint * m_F;
+                m_prevError = m_error;
+
+                if (m_result > m_maximumOutput) {
+                    m_result = m_maximumOutput;
+                } else if (m_result < m_minimumOutput) {
+                    m_result = m_minimumOutput;
+                }
+                pidOutput = m_pidOutput;
+                result = m_result;
+            }
+
+            pidOutput.pidWrite(result);
+        }
+    }
+
+    /**
+     * Set the PID Controller gain parameters.
+     * Set the proportional, integral, and differential coefficients.
+     * @param p Proportional coefficient
+     * @param i Integral coefficient
+     * @param d Differential coefficient
+     */
+    public synchronized void setPID(double p, double i, double d) {
+        m_P = p;
+        m_I = i;
+        m_D = d;
+
+        if (table != null) {
+            table.putNumber("p", p);
+            table.putNumber("i", i);
+            table.putNumber("d", d);
+        }
+    }
+
+    /**
+    * Set the PID Controller gain parameters.
+    * Set the proportional, integral, and differential coefficients.
+    * @param p Proportional coefficient
+    * @param i Integral coefficient
+    * @param d Differential coefficient
+    * @param f Feed forward coefficient
+    */
+    public synchronized void setPID(double p, double i, double d, double f) {
+        m_P = p;
+        m_I = i;
+        m_D = d;
+        m_F = f;
+
+        if (table != null) {
+            table.putNumber("p", p);
+            table.putNumber("i", i);
+            table.putNumber("d", d);
+            table.putNumber("f", f);
+        }
+    }
+
+    /**
+     * Get the Proportional coefficient
+     * @return proportional coefficient
+     */
+    public double getP() {
+        return m_P;
+    }
+
+    /**
+     * Get the Integral coefficient
+     * @return integral coefficient
+     */
+    public double getI() {
+        return m_I;
+    }
+
+    /**
+     * Get the Differential coefficient
+     * @return differential coefficient
+     */
+    public synchronized double getD() {
+        return m_D;
+    }
+
+    /**
+     * Get the Feed forward coefficient
+     * @return feed forward coefficient
+     */
+    public synchronized double getF() {
+        return m_F;
+    }
+
+    /**
+     * Return the current PID result
+     * This is always centered on zero and constrained the the max and min outs
+     * @return the latest calculated output
+     */
+    public synchronized double get() {
+        return m_result;
+    }
+
+    /**
+     *  Set the PID controller to consider the input to be continuous,
+     *  Rather then using the max and min in as constraints, it considers them to
+     *  be the same point and automatically calculates the shortest route to
+     *  the setpoint.
+     * @param continuous Set to true turns on continuous, false turns off continuous
+     */
+    public synchronized void setContinuous(boolean continuous) {
+        m_continuous = continuous;
+    }
+
+    /**
+     *  Set the PID controller to consider the input to be continuous,
+     *  Rather then using the max and min in as constraints, it considers them to
+     *  be the same point and automatically calculates the shortest route to
+     *  the setpoint.
+     */
+    public synchronized void setContinuous() {
+        this.setContinuous(true);
+    }
+
+    /**
+     * Sets the maximum and minimum values expected from the input.
+     *
+     * @param minimumInput the minimum percentage expected from the input
+     * @param maximumInput the maximum percentage expected from the output
+     */
+    public synchronized void setInputRange(double minimumInput, double maximumInput) {
+        if (minimumInput > maximumInput) {
+            throw new BoundaryException("Lower bound is greater than upper bound");
+        }
+        m_minimumInput = minimumInput;
+        m_maximumInput = maximumInput;
+        setSetpoint(m_setpoint);
+    }
+
+    /**
+     * Sets the minimum and maximum values to write.
+     *
+     * @param minimumOutput the minimum percentage to write to the output
+     * @param maximumOutput the maximum percentage to write to the output
+     */
+    public synchronized void setOutputRange(double minimumOutput, double maximumOutput) {
+        if (minimumOutput > maximumOutput) {
+            throw new BoundaryException("Lower bound is greater than upper bound");
+        }
+        m_minimumOutput = minimumOutput;
+        m_maximumOutput = maximumOutput;
+    }
+
+    /**
+     * Set the setpoint for the PIDController
+     * @param setpoint the desired setpoint
+     */
+    public synchronized void setSetpoint(double setpoint) {
+        if (m_maximumInput > m_minimumInput) {
+            if (setpoint > m_maximumInput) {
+                m_setpoint = m_maximumInput;
+            } else if (setpoint < m_minimumInput) {
+                m_setpoint = m_minimumInput;
+            } else {
+                m_setpoint = setpoint;
+            }
+        } else {
+            m_setpoint = setpoint;
+        }
+
+        if (table != null)
+            table.putNumber("setpoint", m_setpoint);
+    }
+
+    /**
+     * Returns the current setpoint of the PIDController
+     * @return the current setpoint
+     */
+    public synchronized double getSetpoint() {
+        return m_setpoint;
+    }
+
+    /**
+     * Returns the current difference of the input from the setpoint
+     * @return the current error
+     */
+    public synchronized double getError() {
+        //return m_error;
+        return getSetpoint() - m_pidInput.pidGet();
+    }
+
+    /**
+     * Set the percentage error which is considered tolerable for use with
+     * OnTarget. (Input of 15.0 = 15 percent)
+     * @param percent error which is tolerable
+     * @deprecated Use setTolerance(Tolerance), i.e. setTolerance(new PIDController.PercentageTolerance(15))
+     */
+    public synchronized void setTolerance(double percent) {
+        m_tolerance = new PercentageTolerance(percent);
+    }
+
+    /** Set the PID tolerance using a Tolerance object.
+     * Tolerance can be specified as a percentage of the range or as an absolute
+     * value. The Tolerance object encapsulates those options in an object. Use it by
+     * creating the type of tolerance that you want to use: setTolerance(new PIDController.AbsoluteTolerance(0.1))
+     * @param tolerance a tolerance object of the right type, e.g. PercentTolerance
+     * or AbsoluteTolerance
+     */
+    private void setTolerance(Tolerance tolerance) {
+        m_tolerance = tolerance;
+    }
+
+    /**
+     * Set the absolute error which is considered tolerable for use with
+     * OnTarget.
+     * @param absolute error which is tolerable in the units of the input object
+     */
+    public synchronized void setAbsoluteTolerance(double absvalue) {
+        m_tolerance = new AbsoluteTolerance(absvalue);
+    }
+
+    /**
+     * Set the percentage error which is considered tolerable for use with
+     * OnTarget. (Input of 15.0 = 15 percent)
+     * @param percent error which is tolerable
+     */
+    public synchronized void setPercentTolerance(double percentage) {
+        m_tolerance = new PercentageTolerance(percentage);
+    }
+
+    /**
+     * Return true if the error is within the percentage of the total input range,
+     * determined by setTolerance. This assumes that the maximum and minimum input
+     * were set using setInput.
+     * @return true if the error is less than the tolerance
+     */
+    public synchronized boolean onTarget() {
+        return m_tolerance.onTarget();
+    }
+
+    /**
+     * Begin running the PIDController
+     */
+    public synchronized void enable() {
+        m_enabled = true;
+
+        if (table != null) {
+            table.putBoolean("enabled", true);
+        }
+    }
+
+    /**
+     * Stop running the PIDController, this sets the output to zero before stopping.
+     */
+    public synchronized void disable() {
+        m_pidOutput.pidWrite(0);
+        m_enabled = false;
+
+        if (table != null) {
+            table.putBoolean("enabled", false);
+        }
+    }
+
+    /**
+     * Return true if PIDController is enabled.
+     */
+    public synchronized boolean isEnable() {
+        return m_enabled;
+    }
+
+    /**
+     * Reset the previous error,, the integral term, and disable the controller.
+     */
+    public synchronized void reset() {
+        disable();
+        m_prevError = 0;
+        m_totalError = 0;
+        m_result = 0;
+    }
+
+    public String getSmartDashboardType() {
+        return "PIDController";
+    }
+
+    private ITableListener listener = new ITableListener() {
+        public void valueChanged(ITable table, String key, Object value, boolean isNew) {
+            if (key.equals("p") || key.equals("i") || key.equals("d") || key.equals("f")) {
+                if (m_P != table.getNumber("p", 0.0) || m_I != table.getNumber("i", 0.0) ||
+                        m_D != table.getNumber("d", 0.0) || m_F != table.getNumber("f", 0.0))
+                    setPID(table.getNumber("p", 0.0), table.getNumber("i", 0.0), table.getNumber("d", 0.0), table.getNumber("f", 0.0));
+            } else if (key.equals("setpoint")) {
+                if (m_setpoint != ((Double) value).doubleValue())
+                    setSetpoint(((Double) value).doubleValue());
+            } else if (key.equals("enabled")) {
+                if (m_enabled != ((Boolean) value).booleanValue()) {
+                    if (((Boolean) value).booleanValue()) {
+                        enable();
+                    } else {
+                        disable();
+                    }
+                }
+            }
+        }
+    };
+    private ITable table;
+    public void initTable(ITable table) {
+        if(this.table!=null)
+            this.table.removeTableListener(listener);
+        this.table = table;
+        if(table!=null) {
+            table.putNumber("p", getP());
+            table.putNumber("i", getI());
+            table.putNumber("d", getD());
+            table.putNumber("f", getF());
+            table.putNumber("setpoint", getSetpoint());
+            table.putBoolean("enabled", isEnable());
+            table.addTableListener(listener, false);
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return table;
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public void updateTable() {
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public void startLiveWindowMode() {
+        disable();
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public void stopLiveWindowMode() {
+    }
+}

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

@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows PIDController to write it's results to its output.
+ * @author dtjones
+ */
+public interface PIDOutput {
+
+    /**
+     * Set the output to the value calculated by PIDController
+     * @param output the value calculated by PIDController
+     */
+    public void pidWrite(double output);
+}

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

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows for PIDController to automatically read from this
+ * object
+ * @author dtjones
+ */
+public interface PIDSource {
+
+    /**
+     * A description for the type of output value to provide to a PIDController
+     */
+    public static class PIDSourceParameter {
+        public final int value;
+        static final int kDistance_val = 0;
+        static final int kRate_val = 1;
+        static final int kAngle_val = 2;
+        public static final PIDSourceParameter kDistance = new PIDSourceParameter(kDistance_val);
+        public static final PIDSourceParameter kRate = new PIDSourceParameter(kRate_val);
+        public static final PIDSourceParameter kAngle = new PIDSourceParameter(kAngle_val);
+
+        private PIDSourceParameter(int value) {
+            this.value = value;
+        }
+    }
+
+    /**
+     * Get the result to use in PIDController
+     * @return the result to use in PIDController
+     */
+    public double pidGet();
+}

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

@@ -0,0 +1,219 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                   */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import java.io.IOException;
+import java.net.URL;
+import java.util.Enumeration;
+import java.util.jar.Manifest;
+
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+
+/**
+ * Implement a Robot Program framework.
+ * The RobotBase class is intended to be subclassed by a user creating a robot program.
+ * Overridden autonomous() and operatorControl() methods are called at the appropriate time
+ * as the match proceeds. In the current implementation, the Autonomous code will run to
+ * completion before the OperatorControl code could start. In the future the Autonomous code
+ * might be spawned as a task, then killed at the end of the Autonomous period.
+ */
+public abstract class RobotBase {
+	private static DriverStation ds;
+	
+    /**
+     * The VxWorks priority that robot code should work at (so Java code should run at)
+     */
+    public static final int ROBOT_TASK_PRIORITY = 101;
+
+    /**
+     * Boolean System property. If true (default), send System.err messages to the driver station.
+     */
+    public final static String ERRORS_TO_DRIVERSTATION_PROP = "first.driverstation.senderrors";
+
+    // TODO: protected final DriverStation m_ds;
+    // TODO: private final Watchdog m_watchdog = Watchdog.getInstance();
+
+    /**
+     * Constructor for a generic robot program.
+     * User code should be placed in the constructor that runs before the Autonomous or Operator
+     * Control period starts. The constructor will run to completion before Autonomous is entered.
+     *
+     * This must be used to ensure that the communications code starts. In the future it would be
+     * nice to put this code into it's own task that loads on boot so ensure that it runs.
+     */
+    protected RobotBase() {
+        // TODO: StartCAPI();
+        // TODO: See if the next line is necessary
+        // Resource.RestartProgram();
+
+//        if (getBooleanProperty(ERRORS_TO_DRIVERSTATION_PROP, true)) {
+//            Utility.sendErrorStreamToDriverStation(true);
+//        }
+        NetworkTable.setServerMode();//must be before b
+        // TODO: m_watchdog.setEnabled(false);
+        NetworkTable.getTable("");  // forces network tables to initialize
+        NetworkTable.getTable("LiveWindow").getSubTable("~STATUS~").putBoolean("LW Enabled", false);
+    }
+
+    /**
+     * Free the resources for a RobotBase class.
+     */
+    public void free() {
+    }
+
+    /**
+     * Check on the overall status of the system.
+     *
+     * @return Is the system active (i.e. PWM motor outputs, etc. enabled)?
+     */
+    public boolean isSystemActive() {
+        // TODO: return m_watchdog.isSystemActive();
+        return true;
+    }
+
+    /**
+     * Return the instance of the Watchdog timer.
+     * Get the watchdog timer so the user program can either disable it or feed it when
+     * necessary.
+     *
+     * @return The Watchdog timer.
+     */
+    // TODO: No watchdog supportr
+    // public Watchdog getWatchdog() {
+    //    return m_watchdog;
+    //}
+    
+
+    /**
+     * @return If the robot is running in simulation.
+     */
+    public static boolean isSimulation() {
+        return true;
+    }
+
+    /**
+     * @return If the robot is running in the real world.
+     */
+    public static boolean isReal() {
+        return false;
+    }
+
+    /**
+     * Determine if the Robot is currently disabled.
+     * @return True if the Robot is currently disabled by the field controls.
+     */
+    public boolean isDisabled() {
+    	return ds.isDisabled();
+    }
+
+    /**
+     * Determine if the Robot is currently enabled.
+     * @return True if the Robot is currently enabled by the field controls.
+     */
+    public boolean isEnabled() {
+    	return ds.isEnabled();
+    }
+
+    /**
+     * Determine if the robot is currently in Autonomous mode.
+     * @return True if the robot is currently operating Autonomously as determined by the field controls.
+     */
+    public boolean isAutonomous() {
+    	return ds.isAutonomous();
+    }
+
+    /**
+     * Determine if the robot is currently in Test mode
+     * @return True if the robot is currently operating in Test mode as determined by the driver station.
+     */
+    public boolean isTest() {
+    	return ds.isTest();
+    }
+
+    /**
+     * Determine if the robot is currently in Operator Control mode.
+     * @return True if the robot is currently operating in Tele-Op mode as determined by the field controls.
+     */
+    public boolean isOperatorControl() {
+    	return ds.isOperatorControl();
+    }
+
+    /**
+     * Indicates if new data is available from the driver station.
+     * @return Has new data arrived over the network since the last time this function was called?
+     */
+    public boolean isNewDataAvailable() {
+    	return ds.isNewControlData();
+    }
+
+    /**
+     * Provide an alternate "main loop" via startCompetition().
+     */
+    public abstract void startCompetition();
+
+    public static boolean getBooleanProperty(String name, boolean defaultValue) {
+        String propVal = System.getProperty(name);
+        if (propVal == null) {
+            return defaultValue;
+        }
+        if (propVal.equalsIgnoreCase("false")) {
+            return false;
+        } else if (propVal.equalsIgnoreCase("true")) {
+            return true;
+        } else {
+            throw new IllegalStateException(propVal);
+        }
+    }
+
+    /**
+     * Starting point for the applications. Starts the OtaServer and then runs
+     * the robot.
+     * @throws javax.microedition.midlet.MIDletStateChangeException
+     */
+    public static void main(String args[]) { // TODO: expose main to teams?{
+        boolean errorOnExit = false;
+
+        ds = DriverStation.getInstance();
+
+		String robotName = "";
+		Enumeration<URL> resources = null;
+		try {
+			resources = RobotBase.class.getClassLoader().getResources("META-INF/MANIFEST.MF");
+		} catch (IOException e) {e.printStackTrace();}
+		while (resources != null && resources.hasMoreElements()) {
+		    try {
+		    	Manifest manifest = new Manifest(resources.nextElement().openStream());
+		    	robotName = manifest.getMainAttributes().getValue("Robot-Class");
+		    } catch (IOException e) {e.printStackTrace();}
+		}
+
+        RobotBase robot;
+		try {
+			robot = (RobotBase) Class.forName(robotName).newInstance();
+		} catch (InstantiationException|IllegalAccessException|ClassNotFoundException e) {
+			System.err.println("WARNING: Robots don't quit!");
+			System.err.println("ERROR: Could not instantiate robot "+robotName+"!");
+			return;
+		}
+
+        try {
+            robot.startCompetition();
+        } catch (Throwable t) {
+            t.printStackTrace();
+            errorOnExit = true;
+        } finally {
+            // startCompetition never returns unless exception occurs....
+            System.err.println("WARNING: Robots don't quit!");
+            if (errorOnExit) {
+                System.err.println("---> The startCompetition() method (or methods called by it) should have handled the exception above.");
+            } else {
+                System.err.println("---> Unexpected return from startCompetition() method.");
+            }
+        }
+    }
+}

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

@@ -0,0 +1,716 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj;
+
+/**
+ * Utility class for handling Robot drive based on a definition of the motor configuration.
+ * The robot drive class handles basic driving for a robot. Currently, 2 and 4 motor standard
+ * drive trains are supported. In the future other drive types like swerve and meccanum might
+ * be implemented. Motor channel numbers are passed supplied on creation of the class. Those are
+ * used for either the drive function (intended for hand created drive code, such as autonomous)
+ * or with the Tank/Arcade functions intended to be used for Operator Control driving.
+ */
+public class RobotDrive implements MotorSafety {
+
+    protected MotorSafetyHelper m_safetyHelper;
+
+    /**
+     * The location of a motor on the robot for the purpose of driving
+     */
+    public static class MotorType {
+
+        /**
+         * The integer value representing this enumeration
+         */
+        public final int value;
+        static final int kFrontLeft_val = 0;
+        static final int kFrontRight_val = 1;
+        static final int kRearLeft_val = 2;
+        static final int kRearRight_val = 3;
+        /**
+         * motortype: front left
+         */
+        public static final MotorType kFrontLeft = new MotorType(kFrontLeft_val);
+        /**
+         * motortype: front right
+         */
+        public static final MotorType kFrontRight = new MotorType(kFrontRight_val);
+        /**
+         * motortype: rear left
+         */
+        public static final MotorType kRearLeft = new MotorType(kRearLeft_val);
+        /**
+         * motortype: rear right
+         */
+        public static final MotorType kRearRight = new MotorType(kRearRight_val);
+
+        private MotorType(int value) {
+            this.value = value;
+        }
+    }
+    public static final double kDefaultExpirationTime = 0.1;
+    public static final double kDefaultSensitivity = 0.5;
+    public static final double kDefaultMaxOutput = 1.0;
+    protected static final int kMaxNumberOfMotors = 4;
+    protected final int m_invertedMotors[] = new int[4];
+    protected double m_sensitivity;
+    protected double m_maxOutput;
+    protected SpeedController m_frontLeftMotor;
+    protected SpeedController m_frontRightMotor;
+    protected SpeedController m_rearLeftMotor;
+    protected SpeedController m_rearRightMotor;
+    protected boolean m_allocatedSpeedControllers;
+    protected boolean m_isCANInitialized = true;
+    protected static boolean kArcadeRatioCurve_Reported = false;
+    protected static boolean kTank_Reported = false;
+    protected static boolean kArcadeStandard_Reported = false;
+    protected static boolean kMecanumCartesian_Reported = false;
+    protected static boolean kMecanumPolar_Reported = false;
+
+    /** Constructor for RobotDrive with 2 motors specified with channel numbers.
+     * Set up parameters for a two wheel drive system where the
+     * left and right motor pwm channels are specified in the call.
+     * This call assumes Jaguars for controlling the motors.
+     * @param leftMotorChannel The PWM channel number on the default digital module that drives the left motor.
+     * @param rightMotorChannel The PWM channel number on the default digital module that drives the right motor.
+     */
+    public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
+        m_sensitivity = kDefaultSensitivity;
+        m_maxOutput = kDefaultMaxOutput;
+        m_frontLeftMotor = null;
+        m_rearLeftMotor = new Talon(leftMotorChannel);
+        m_frontRightMotor = null;
+        m_rearRightMotor = new Talon(rightMotorChannel);
+        for (int i = 0; i < kMaxNumberOfMotors; i++) {
+            m_invertedMotors[i] = 1;
+        }
+        m_allocatedSpeedControllers = true;
+        setupMotorSafety();
+        drive(0, 0);
+    }
+
+    /**
+     * Constructor for RobotDrive with 4 motors specified with channel numbers.
+     * Set up parameters for a four wheel drive system where all four motor
+     * pwm channels are specified in the call.
+     * This call assumes Jaguars for controlling the motors.
+     * @param frontLeftMotor Front left motor channel number on the default digital module
+     * @param rearLeftMotor Rear Left motor channel number on the default digital module
+     * @param frontRightMotor Front right motor channel number on the default digital module
+     * @param rearRightMotor Rear Right motor channel number on the default digital module
+     */
+    public RobotDrive(final int frontLeftMotor, final int rearLeftMotor,
+                      final int frontRightMotor, final int rearRightMotor) {
+        m_sensitivity = kDefaultSensitivity;
+        m_maxOutput = kDefaultMaxOutput;
+        m_rearLeftMotor = new Talon(rearLeftMotor);
+        m_rearRightMotor = new Talon(rearRightMotor);
+        m_frontLeftMotor = new Talon(frontLeftMotor);
+        m_frontRightMotor = new Talon(frontRightMotor);
+        for (int i = 0; i < kMaxNumberOfMotors; i++) {
+            m_invertedMotors[i] = 1;
+        }
+        m_allocatedSpeedControllers = true;
+        setupMotorSafety();
+        drive(0, 0);
+    }
+
+    /**
+     * Constructor for RobotDrive with 2 motors specified as SpeedController objects.
+     * The SpeedController version of the constructor enables programs to use the RobotDrive classes with
+     * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
+     * the curve to suit motor bias or dead-band elimination.
+     * @param leftMotor The left SpeedController object used to drive the robot.
+     * @param rightMotor the right SpeedController object used to drive the robot.
+     */
+    public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
+        if (leftMotor == null || rightMotor == null) {
+            m_rearLeftMotor = m_rearRightMotor = null;
+            throw new NullPointerException("Null motor provided");
+        }
+        m_frontLeftMotor = null;
+        m_rearLeftMotor = leftMotor;
+        m_frontRightMotor = null;
+        m_rearRightMotor = rightMotor;
+        m_sensitivity = kDefaultSensitivity;
+        m_maxOutput = kDefaultMaxOutput;
+        for (int i = 0; i < kMaxNumberOfMotors; i++) {
+            m_invertedMotors[i] = 1;
+        }
+        m_allocatedSpeedControllers = false;
+        setupMotorSafety();
+        drive(0, 0);
+    }
+
+    /**
+     * Constructor for RobotDrive with 4 motors specified as SpeedController objects.
+     * Speed controller input version of RobotDrive (see previous comments).
+     * @param rearLeftMotor The back left SpeedController object used to drive the robot.
+     * @param frontLeftMotor The front left SpeedController object used to drive the robot
+     * @param rearRightMotor The back right SpeedController object used to drive the robot.
+     * @param frontRightMotor The front right SpeedController object used to drive the robot.
+     */
+    public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
+                      SpeedController frontRightMotor, SpeedController rearRightMotor) {
+        if (frontLeftMotor == null || rearLeftMotor == null || frontRightMotor == null || rearRightMotor == null) {
+            m_frontLeftMotor = m_rearLeftMotor = m_frontRightMotor = m_rearRightMotor = null;
+            throw new NullPointerException("Null motor provided");
+        }
+        m_frontLeftMotor = frontLeftMotor;
+        m_rearLeftMotor = rearLeftMotor;
+        m_frontRightMotor = frontRightMotor;
+        m_rearRightMotor = rearRightMotor;
+        m_sensitivity = kDefaultSensitivity;
+        m_maxOutput = kDefaultMaxOutput;
+        for (int i = 0; i < kMaxNumberOfMotors; i++) {
+            m_invertedMotors[i] = 1;
+        }
+        m_allocatedSpeedControllers = false;
+        setupMotorSafety();
+        drive(0, 0);
+    }
+
+    /**
+     * Drive the motors at "speed" and "curve".
+     *
+     * The speed and curve are -1.0 to +1.0 values where 0.0 represents stopped and
+     * not turning. The algorithm for adding in the direction attempts to provide a constant
+     * turn radius for differing speeds.
+     *
+     * This function will most likely be used in an autonomous routine.
+     *
+     * @param outputMagnitude The forward component of the output magnitude to send to the motors.
+     * @param curve The rate of turn, constant for different forward speeds.
+     */
+    public void drive(double outputMagnitude, double curve) {
+        double leftOutput, rightOutput;
+
+        if(!kArcadeRatioCurve_Reported) {
+            // UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeRatioCurve);
+            kArcadeRatioCurve_Reported = true;
+        }
+        if (curve < 0) {
+            double value = Math.log(-curve);
+            double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+            if (ratio == 0) {
+                ratio = .0000000001;
+            }
+            leftOutput = outputMagnitude / ratio;
+            rightOutput = outputMagnitude;
+        } else if (curve > 0) {
+            double value = Math.log(curve);
+            double ratio = (value - m_sensitivity) / (value + m_sensitivity);
+            if (ratio == 0) {
+                ratio = .0000000001;
+            }
+            leftOutput = outputMagnitude;
+            rightOutput = outputMagnitude / ratio;
+        } else {
+            leftOutput = outputMagnitude;
+            rightOutput = outputMagnitude;
+        }
+        setLeftRightMotorOutputs(leftOutput, rightOutput);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * drive the robot using two joystick inputs. The Y-axis will be selected from
+     * each Joystick object.
+     * @param leftStick The joystick to control the left side of the robot.
+     * @param rightStick The joystick to control the right side of the robot.
+     */
+    public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
+        if (leftStick == null || rightStick == null) {
+            throw new NullPointerException("Null HID provided");
+        }
+        tankDrive(leftStick.getY(), rightStick.getY(), true);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * drive the robot using two joystick inputs. The Y-axis will be selected from
+     * each Joystick object.
+     * @param leftStick The joystick to control the left side of the robot.
+     * @param rightStick The joystick to control the right side of the robot.
+     * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+     */
+    public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
+        if (leftStick == null || rightStick == null) {
+            throw new NullPointerException("Null HID provided");
+        }
+        tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * This function lets you pick the axis to be used on each Joystick object for the left
+     * and right sides of the robot.
+     * @param leftStick The Joystick object to use for the left side of the robot.
+     * @param leftAxis The axis to select on the left side Joystick object.
+     * @param rightStick The Joystick object to use for the right side of the robot.
+     * @param rightAxis The axis to select on the right side Joystick object.
+     */
+    public void tankDrive(GenericHID leftStick, final int leftAxis,
+                          GenericHID rightStick, final int rightAxis) {
+        if (leftStick == null || rightStick == null) {
+            throw new NullPointerException("Null HID provided");
+        }
+        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * This function lets you pick the axis to be used on each Joystick object for the left
+     * and right sides of the robot.
+     * @param leftStick The Joystick object to use for the left side of the robot.
+     * @param leftAxis The axis to select on the left side Joystick object.
+     * @param rightStick The Joystick object to use for the right side of the robot.
+     * @param rightAxis The axis to select on the right side Joystick object.
+     * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+     */
+    public void tankDrive(GenericHID leftStick, final int leftAxis,
+                          GenericHID rightStick, final int rightAxis, boolean squaredInputs) {
+        if (leftStick == null || rightStick == null) {
+            throw new NullPointerException("Null HID provided");
+        }
+        tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * This function lets you directly provide joystick values from any source.
+     * @param leftValue The value of the left stick.
+     * @param rightValue The value of the right stick.
+     * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+     */
+    public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
+
+        if(!kTank_Reported) {
+            // UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_Tank);
+            kTank_Reported = true;
+        }
+
+        // square the inputs (while preserving the sign) to increase fine control while permitting full power
+        leftValue = limit(leftValue);
+        rightValue = limit(rightValue);
+        if(squaredInputs) {
+            if (leftValue >= 0.0) {
+                leftValue = (leftValue * leftValue);
+            } else {
+                leftValue = -(leftValue * leftValue);
+            }
+            if (rightValue >= 0.0) {
+                rightValue = (rightValue * rightValue);
+            } else {
+                rightValue = -(rightValue * rightValue);
+            }
+        }
+        setLeftRightMotorOutputs(leftValue, rightValue);
+    }
+
+    /**
+     * Provide tank steering using the stored robot configuration.
+     * This function lets you directly provide joystick values from any source.
+     * @param leftValue The value of the left stick.
+     * @param rightValue The value of the right stick.
+     */
+    public void tankDrive(double leftValue, double rightValue) {
+        tankDrive(leftValue, rightValue, true);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
+     * for the rotate value.
+     * (Should add more information here regarding the way that arcade drive works.)
+     * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+     * for forwards/backwards and the X-axis will be selected for rotation rate.
+     * @param squaredInputs If true, the sensitivity will be decreased for small values
+     */
+    public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
+        // simply call the full-featured arcadeDrive with the appropriate values
+        arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * Given a single Joystick, the class assumes the Y axis for the move value and the X axis
+     * for the rotate value.
+     * (Should add more information here regarding the way that arcade drive works.)
+     * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
+     * for forwards/backwards and the X-axis will be selected for rotation rate.
+     */
+    public void arcadeDrive(GenericHID stick) {
+        this.arcadeDrive(stick, true);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * Given two joystick instances and two axis, compute the values to send to either two
+     * or four motors.
+     * @param moveStick The Joystick object that represents the forward/backward direction
+     * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
+     * @param rotateStick The Joystick object that represents the rotation value
+     * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
+     * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
+     */
+    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
+                            GenericHID rotateStick, final int rotateAxis,
+                            boolean squaredInputs) {
+        double moveValue = moveStick.getRawAxis(moveAxis);
+        double rotateValue = rotateStick.getRawAxis(rotateAxis);
+
+        arcadeDrive(moveValue, rotateValue, squaredInputs);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * Given two joystick instances and two axis, compute the values to send to either two
+     * or four motors.
+     * @param moveStick The Joystick object that represents the forward/backward direction
+     * @param moveAxis The axis on the moveStick object to use for forwards/backwards (typically Y_AXIS)
+     * @param rotateStick The Joystick object that represents the rotation value
+     * @param rotateAxis The axis on the rotation object to use for the rotate right/left (typically X_AXIS)
+     */
+    public void arcadeDrive(GenericHID moveStick, final int moveAxis,
+                            GenericHID rotateStick, final int rotateAxis) {
+        this.arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * This function lets you directly provide joystick values from any source.
+     * @param moveValue The value to use for forwards/backwards
+     * @param rotateValue The value to use for the rotate right/left
+     * @param squaredInputs If set, decreases the sensitivity at low speeds
+     */
+    public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
+        // local variables to hold the computed PWM values for the motors
+        if(!kArcadeStandard_Reported) {
+            // UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_ArcadeStandard);
+            kArcadeStandard_Reported = true;
+        }
+
+        double leftMotorSpeed;
+        double rightMotorSpeed;
+
+        moveValue = limit(moveValue);
+        rotateValue = limit(rotateValue);
+
+        if (squaredInputs) {
+            // square the inputs (while preserving the sign) to increase fine control while permitting full power
+            if (moveValue >= 0.0) {
+                moveValue = (moveValue * moveValue);
+            } else {
+                moveValue = -(moveValue * moveValue);
+            }
+            if (rotateValue >= 0.0) {
+                rotateValue = (rotateValue * rotateValue);
+            } else {
+                rotateValue = -(rotateValue * rotateValue);
+            }
+        }
+
+        if (moveValue > 0.0) {
+            if (rotateValue > 0.0) {
+                leftMotorSpeed = moveValue - rotateValue;
+                rightMotorSpeed = Math.max(moveValue, rotateValue);
+            } else {
+                leftMotorSpeed = Math.max(moveValue, -rotateValue);
+                rightMotorSpeed = moveValue + rotateValue;
+            }
+        } else {
+            if (rotateValue > 0.0) {
+                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
+                rightMotorSpeed = moveValue + rotateValue;
+            } else {
+                leftMotorSpeed = moveValue - rotateValue;
+                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
+            }
+        }
+
+        setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
+    }
+
+    /**
+     * Arcade drive implements single stick driving.
+     * This function lets you directly provide joystick values from any source.
+     * @param moveValue The value to use for fowards/backwards
+     * @param rotateValue The value to use for the rotate right/left
+     */
+    public void arcadeDrive(double moveValue, double rotateValue) {
+        this.arcadeDrive(moveValue, rotateValue, true);
+    }
+
+    /**
+     * Drive method for Mecanum wheeled robots.
+     *
+     * A method for driving with Mecanum wheeled robots. There are 4 wheels
+     * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
+     * When looking at the wheels from the top, the roller axles should form an X across the robot.
+     *
+     * This is designed to be directly driven by joystick axes.
+     *
+     * @param x The speed that the robot should drive in the X direction. [-1.0..1.0]
+     * @param y The speed that the robot should drive in the Y direction.
+     * This input is inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
+     * @param rotation The rate of rotation for the robot that is completely independent of
+     * the translation. [-1.0..1.0]
+     * @param gyroAngle The current angle reading from the gyro.  Use this to implement field-oriented controls.
+     */
+    public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
+        if(!kMecanumCartesian_Reported) {
+            // UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumCartesian);
+            kMecanumCartesian_Reported = true;
+        }
+        double xIn = x;
+        double yIn = y;
+        // Negate y for the joystick.
+        yIn = -yIn;
+        // Compenstate for gyro angle.
+        double rotated[] = rotateVector(xIn, yIn, gyroAngle);
+        xIn = rotated[0];
+        yIn = rotated[1];
+
+        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
+        wheelSpeeds[MotorType.kFrontLeft_val] = xIn + yIn + rotation;
+        wheelSpeeds[MotorType.kFrontRight_val] = -xIn + yIn - rotation;
+        wheelSpeeds[MotorType.kRearLeft_val] = -xIn + yIn + rotation;
+        wheelSpeeds[MotorType.kRearRight_val] = xIn + yIn - rotation;
+
+        normalize(wheelSpeeds);
+
+        byte syncGroup = (byte)0x80;
+
+        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
+        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
+        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
+        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
+
+        if (m_safetyHelper != null) m_safetyHelper.feed();
+    }
+
+    /**
+     * Drive method for Mecanum wheeled robots.
+     *
+     * A method for driving with Mecanum wheeled robots. There are 4 wheels
+     * on the robot, arranged so that the front and back wheels are toed in 45 degrees.
+     * When looking at the wheels from the top, the roller axles should form an X across the robot.
+     *
+     * @param magnitude The speed that the robot should drive in a given direction.
+     * @param direction The direction the robot should drive in degrees. The direction and maginitute are
+     * independent of the rotation rate.
+     * @param rotation The rate of rotation for the robot that is completely independent of
+     * the magnitute or direction. [-1.0..1.0]
+     */
+    public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
+        if(!kMecanumPolar_Reported) {
+            // UsageReporting.report(tResourceType.kResourceType_RobotDrive, getNumMotors(), tInstances.kRobotDrive_MecanumPolar);
+            kMecanumPolar_Reported = true;
+        }
+        double frontLeftSpeed, rearLeftSpeed, frontRightSpeed, rearRightSpeed;
+        // Normalized for full power along the Cartesian axes.
+        magnitude = limit(magnitude) * Math.sqrt(2.0);
+        // The rollers are at 45 degree angles.
+        double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
+        double cosD = Math.cos(dirInRad);
+        double sinD = Math.sin(dirInRad);
+
+        double wheelSpeeds[] = new double[kMaxNumberOfMotors];
+        wheelSpeeds[MotorType.kFrontLeft_val] = (sinD * magnitude + rotation);
+        wheelSpeeds[MotorType.kFrontRight_val] = (cosD * magnitude - rotation);
+        wheelSpeeds[MotorType.kRearLeft_val] = (cosD * magnitude + rotation);
+        wheelSpeeds[MotorType.kRearRight_val] = (sinD * magnitude - rotation);
+
+        normalize(wheelSpeeds);
+
+        byte syncGroup = (byte)0x80;
+
+        m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft_val] * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
+        m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight_val] * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
+        m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft_val] * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
+        m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight_val] * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
+
+        if (m_safetyHelper != null) m_safetyHelper.feed();
+    }
+
+    /**
+     * Holonomic Drive method for Mecanum wheeled robots.
+     *
+     * This is an alias to mecanumDrive_Polar() for backward compatability
+     *
+     * @param magnitude The speed that the robot should drive in a given direction.  [-1.0..1.0]
+     * @param direction The direction the robot should drive. The direction and maginitute are
+     * independent of the rotation rate.
+     * @param rotation The rate of rotation for the robot that is completely independent of
+     * the magnitute or direction.  [-1.0..1.0]
+     */
+    void holonomicDrive(float magnitude, float direction, float rotation) {
+        mecanumDrive_Polar(magnitude, direction, rotation);
+    }
+
+    /** Set the speed of the right and left motors.
+     * This is used once an appropriate drive setup function is called such as
+     * twoWheelDrive(). The motors are set to "leftSpeed" and "rightSpeed"
+     * and includes flipping the direction of one side for opposing motors.
+     * @param leftOutput The speed to send to the left side of the robot.
+     * @param rightOutput The speed to send to the right side of the robot.
+     */
+    public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
+        if (m_rearLeftMotor == null || m_rearRightMotor == null) {
+            throw new NullPointerException("Null motor provided");
+        }
+
+        byte syncGroup = (byte)0x80;
+
+        if (m_frontLeftMotor != null) {
+            m_frontLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kFrontLeft_val] * m_maxOutput, syncGroup);
+        }
+        m_rearLeftMotor.set(limit(leftOutput) * m_invertedMotors[MotorType.kRearLeft_val] * m_maxOutput, syncGroup);
+
+        if (m_frontRightMotor != null) {
+            m_frontRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kFrontRight_val] * m_maxOutput, syncGroup);
+        }
+        m_rearRightMotor.set(-limit(rightOutput) * m_invertedMotors[MotorType.kRearRight_val] * m_maxOutput, syncGroup);
+
+        if (m_safetyHelper != null) m_safetyHelper.feed();
+    }
+
+    /**
+     * Limit motor values to the -1.0 to +1.0 range.
+     */
+    protected static double limit(double num) {
+        if (num > 1.0) {
+            return 1.0;
+        }
+        if (num < -1.0) {
+            return -1.0;
+        }
+        return num;
+    }
+
+    /**
+     * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+     */
+    protected static void normalize(double wheelSpeeds[]) {
+        double maxMagnitude = Math.abs(wheelSpeeds[0]);
+        int i;
+        for (i=1; i<kMaxNumberOfMotors; i++) {
+            double temp = Math.abs(wheelSpeeds[i]);
+            if (maxMagnitude < temp) maxMagnitude = temp;
+        }
+        if (maxMagnitude > 1.0) {
+            for (i=0; i<kMaxNumberOfMotors; i++) {
+                wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
+            }
+        }
+    }
+
+    /**
+     * Rotate a vector in Cartesian space.
+     */
+    protected static double[] rotateVector(double x, double y, double angle) {
+        double cosA = Math.cos(angle * (3.14159 / 180.0));
+        double sinA = Math.sin(angle * (3.14159 / 180.0));
+        double out[] = new double[2];
+        out[0] = x * cosA - y * sinA;
+        out[1] = x * sinA + y * cosA;
+        return out;
+    }
+
+    /**
+     * Invert a motor direction.
+     * This is used when a motor should run in the opposite direction as the drive
+     * code would normally run it. Motors that are direct drive would be inverted, the
+     * drive code assumes that the motors are geared with one reversal.
+     * @param motor The motor index to invert.
+     * @param isInverted True if the motor should be inverted when operated.
+     */
+    public void setInvertedMotor(MotorType motor, boolean isInverted) {
+        m_invertedMotors[motor.value] = isInverted ? -1 : 1;
+    }
+
+    /**
+     * Set the turning sensitivity.
+     *
+     * This only impacts the drive() entry-point.
+     * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
+     */
+    public void setSensitivity(double sensitivity) {
+        m_sensitivity = sensitivity;
+    }
+
+    /**
+     * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than PercentVbus.
+     * @param maxOutput Multiplied with the output percentage computed by the drive functions.
+     */
+    public void setMaxOutput(double maxOutput) {
+        m_maxOutput = maxOutput;
+    }
+
+
+    /**
+     * Free the speed controllers if they were allocated locally
+     */
+    public void free() {
+        
+    }
+
+    public void setExpiration(double timeout) {
+        m_safetyHelper.setExpiration(timeout);
+    }
+
+    public double getExpiration() {
+        return m_safetyHelper.getExpiration();
+    }
+
+    public boolean isAlive() {
+        return m_safetyHelper.isAlive();
+    }
+
+    public boolean isSafetyEnabled() {
+        return m_safetyHelper.isSafetyEnabled();
+    }
+
+    public void setSafetyEnabled(boolean enabled) {
+        m_safetyHelper.setSafetyEnabled(enabled);
+    }
+
+    public String getDescription() {
+        return "Robot Drive";
+    }
+
+    public void stopMotor() {
+        if (m_frontLeftMotor != null) {
+            m_frontLeftMotor.set(0.0);
+        }
+        if (m_frontRightMotor != null) {
+            m_frontRightMotor.set(0.0);
+        }
+        if (m_rearLeftMotor != null) {
+            m_rearLeftMotor.set(0.0);
+        }
+        if (m_rearRightMotor != null) {
+            m_rearRightMotor.set(0.0);
+        }
+    }
+
+    private void setupMotorSafety() {
+        m_safetyHelper = new MotorSafetyHelper(this);
+        m_safetyHelper.setExpiration(kDefaultExpirationTime);
+        m_safetyHelper.setSafetyEnabled(true);
+    }
+
+    protected int getNumMotors() {
+        int motors = 0;
+        if (m_frontLeftMotor != null) motors++;
+        if (m_frontRightMotor != null) motors++;
+        if (m_rearLeftMotor != null) motors++;
+        if (m_rearRightMotor != null) motors++;
+        return motors;
+    }
+}

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

@@ -0,0 +1,26 @@
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.tables.ITable;
+
+
+/**
+ * The base interface for objects that can be sent over the network
+ * through network tables.
+ */
+public interface Sendable {
+    /**
+     * Initializes a table for this sendable object.
+     * @param subtable The table to put the values in.
+     */
+    public void initTable(ITable subtable);
+
+    /**
+     * @return the table that is currently associated with the sendable
+     */
+    public ITable getTable();
+
+    /**
+     * @return the string representation of the named data type that will be used by the smart dashboard for this sendable
+     */
+    public String getSmartDashboardType();
+}

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

@@ -0,0 +1,260 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Base class for all sensors.
+ * Stores most recent status information as well as containing utility functions for checking
+ * channels and error processing.
+ * 
+ * XXX: Wait, there's no exception thrown if we try to allocate a non-existent module?  It that behavior correct?
+ */
+public abstract class SensorBase { // TODO: Refactor
+
+	// TODO: Move this to the HAL
+	
+    /**
+     * Ticks per microsecond
+     */
+    public static final int kSystemClockTicksPerMicrosecond = 40;
+    /**
+     * Number of digital channels per digital sidecar
+     */
+    public static final int kDigitalChannels = 14;
+    /**
+     * Number of digital modules
+     * XXX: This number is incorrect.  We need to find the correct number.
+     */
+    public static final int kDigitalModules = 2;
+    /**
+     * Number of analog channels per module
+     */
+    public static final int kAnalogChannels = 8;
+    /**
+     * Number of analog modules
+     */
+    public static final int kAnalogModules = 2;
+    /**
+     * Number of solenoid channels per module
+     */
+    public static final int kSolenoidChannels = 8;
+    /**
+     * Number of analog modules
+     */
+    public static final int kSolenoidModules = 2;
+    /**
+     * Number of PWM channels per sidecar
+     */
+    public static final int kPwmChannels = 10;
+    /**
+     * Number of relay channels per sidecar
+     */
+    public static final int kRelayChannels = 8;
+
+    private static int m_defaultAnalogModule = 1;
+    private static int m_defaultDigitalModule = 1;
+    private static int m_defaultSolenoidModule = 1;
+
+    /**
+     * Creates an instance of the sensor base and gets an FPGA handle
+     */
+    public SensorBase() {
+    }
+
+    /**
+     * Sets the default Digital Module.
+     * This sets the default digital module to use for objects that are created without
+     * specifying the digital module in the constructor. The default module is initialized
+     * to the first module in the chassis.
+     *
+     * @param moduleNumber The number of the digital module to use.
+     */
+    public static void setDefaultDigitalModule(final int moduleNumber) {
+        checkDigitalModule(moduleNumber);
+        SensorBase.m_defaultDigitalModule = moduleNumber;
+    }
+
+    /**
+     * Sets the default Analog module.
+     * This sets the default analog module to use for objects that are created without
+     * specifying the analog module in the constructor. The default module is initialized
+     * to the first module in the chassis.
+     *
+     * @param moduleNumber The number of the analog module to use.
+     */
+    public static void setDefaultAnalogModule(final int moduleNumber) {
+        checkAnalogModule(moduleNumber);
+        SensorBase.m_defaultAnalogModule = moduleNumber;
+    }
+
+    /**
+     * Set the default location for the Solenoid (9472) module.
+     *
+     * @param moduleNumber The number of the solenoid module to use.
+     */
+    public static void setDefaultSolenoidModule(final int moduleNumber) {
+        checkSolenoidModule(moduleNumber);
+        SensorBase.m_defaultSolenoidModule = moduleNumber;
+    }
+
+    /**
+     * Check that the digital module number is valid.
+     * Module numbers are 1 or 2 (they are no longer real cRIO slots).
+     *
+     * @param moduleNumber The digital module module number to check.
+     */
+    protected static void checkDigitalModule(final int moduleNumber) {
+    	// TODO: fix
+        if(moduleNumber == 1 || moduleNumber == 2)
+            System.err.println("Digital module " + moduleNumber + " is not present.");
+    }
+
+    /**
+     * Check that the digital module number is valid.
+     * Module numbers are 1 or 2 (they are no longer real cRIO slots).
+     *
+     * @param moduleNumber The digital module module number to check.
+     */
+    protected static void checkRelayModule(final int moduleNumber) {
+        checkDigitalModule(moduleNumber);
+    }
+
+    /**
+     * Check that the digital module number is valid.
+     * Module numbers are 1 or 2 (they are no longer real cRIO slots).
+     *
+     * @param moduleNumber The digital module module number to check.
+     */
+    protected static void checkPWMModule(final int moduleNumber) {
+        checkDigitalModule(moduleNumber);
+    }
+
+    /**
+     * Check that the analog module number is valid.
+     * Module numbers are 1 or 2 (they are no longer real cRIO slots).
+     *
+     * @param moduleNumber The analog module module number to check.
+     */
+    protected static void checkAnalogModule(final int moduleNumber) {
+    	// TODO: fix
+        if(moduleNumber == 1 || moduleNumber == 2) {
+            System.err.println("Analog module " + moduleNumber + " is not present.");
+        }
+    }
+
+    /**
+     * Verify that the solenoid module is correct.
+     * Module numbers are 1 or 2 (they are no longer real cRIO slots).
+     *
+     * @param moduleNumber The solenoid module module number to check.
+     */
+    protected static void checkSolenoidModule(final int moduleNumber) {
+    	// TODO: fix
+        if(moduleNumber == 1 || moduleNumber == 2) {
+            System.err.println("Solenoid module " + moduleNumber + " is not present.");
+        }
+    }
+
+    /**
+     * Check that the digital channel number is valid.
+     * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+     * 1-based.
+     *
+     * @param channel The channel number to check.
+     */
+    protected static void checkDigitalChannel(final int channel) {
+        if (channel <= 0 || channel > kDigitalChannels) {
+            System.err.println("Requested digital channel number is out of range.");
+        }
+    }
+
+    /**
+     * Check that the digital channel number is valid.
+     * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+     * 1-based.
+     *
+     * @param channel The channel number to check.
+     */
+    protected static void checkRelayChannel(final int channel) {
+        if (channel <= 0 || channel > kRelayChannels) {
+            System.err.println("Requested relay channel number is out of range.");
+            throw new IndexOutOfBoundsException("Requested relay channel number is out of range.");
+        }
+    }
+
+    /**
+     * Check that the digital channel number is valid.
+     * Verify that the channel number is one of the legal channel numbers. Channel numbers are
+     * 1-based.
+     *
+     * @param channel The channel number to check.
+     */
+    protected static void checkPWMChannel(final int channel) {
+        if (channel <= 0 || channel > kPwmChannels) {
+            System.err.println("Requested PWM channel number is out of range.");
+            throw new IndexOutOfBoundsException("Requested PWM channel number is out of range.");
+        }
+    }
+
+    /**
+     * Check that the analog channel number is value.
+     * Verify that the analog channel number is one of the legal channel numbers. Channel numbers
+     * are 1-based.
+     *
+     * @param channel The channel number to check.
+     */
+    protected static void checkAnalogChannel(final int channel) {
+        if (channel <= 0 || channel > kAnalogChannels) {
+            System.err.println("Requested analog channel number is out of range.");
+        }
+    }
+
+    /**
+     * Verify that the solenoid channel number is within limits.  Channel numbers
+     * are 1-based.
+     *
+     * @param channel The channel number to check.
+     */
+    protected static void checkSolenoidChannel(final int channel) {
+        if (channel <= 0 || channel > kSolenoidChannels) {
+            System.err.println("Requested solenoid channel number is out of range.");
+        }
+    }
+
+    /**
+     * Get the number of the default analog module.
+     *
+     * @return The number of the default analog module.
+     */
+    public static int getDefaultAnalogModule() {
+        return SensorBase.m_defaultAnalogModule;
+    }
+
+    /**
+     * Get the number of the default analog module.
+     *
+     * @return The number of the default analog module.
+     */
+    public static int getDefaultDigitalModule() {
+        return SensorBase.m_defaultDigitalModule;
+    }
+
+    /**
+     * Get the number of the default analog module.
+     *
+     * @return The number of the default analog module.
+     */
+    public static int getDefaultSolenoidModule() {
+        return SensorBase.m_defaultSolenoidModule;
+    }
+
+    /**
+     * Free the resources used by this object
+     */
+    public void free() {}
+}

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

@@ -0,0 +1,162 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+
+/**
+ * A simple robot base class that knows the standard FRC competition states (disabled, autonomous, or operator controlled).
+ *
+ * You can build a simple robot program off of this by overriding the
+ * robotinit(), disabled(), autonomous() and operatorControl() methods. The startCompetition() method will calls these methods
+ * (sometimes repeatedly).
+ * depending on the state of the competition.
+ *
+ * Alternatively you can override the robotMain() method and manage all aspects of the robot yourself.
+ */
+public class SimpleRobot extends RobotBase {
+
+    private boolean m_robotMainOverridden;
+
+    /**
+     * Create a new SimpleRobot
+     */
+    public SimpleRobot() {
+        super();
+        m_robotMainOverridden = true;
+    }
+
+    /**
+     * Robot-wide initialization code should go here.
+     *
+     * Users should override this method for default Robot-wide initialization which will
+     * be called when the robot is first powered on.
+     *
+     * Called exactly 1 time when the competition starts.
+     */
+    protected void robotInit() {
+        System.out.println("Default robotInit() method running, consider providing your own");
+    }
+
+    /**
+     * Disabled should go here.
+     * Users should overload this method to run code that should run while the field is
+     * disabled.
+     *
+     * Called once each time the robot enters the disabled state.
+     */
+    protected void disabled() {
+        System.out.println("Default disabled() method running, consider providing your own");
+    }
+
+    /**
+     * Autonomous should go here.
+     * Users should add autonomous code to this method that should run while the field is
+     * in the autonomous period.
+     *
+     * Called once each time the robot enters the autonomous state.
+     */
+    public void autonomous() {
+        System.out.println("Default autonomous() method running, consider providing your own");
+    }
+
+    /**
+     * Operator control (tele-operated) code should go here.
+     * Users should add Operator Control code to this method that should run while the field is
+     * in the Operator Control (tele-operated) period.
+     *
+     * Called once each time the robot enters the operator-controlled state.
+     */
+    public void operatorControl() {
+        System.out.println("Default operatorControl() method running, consider providing your own");
+    }
+
+    /**
+     * Test code should go here.
+     * Users should add test code to this method that should run while the robot is in test mode.
+     */
+    public void test() {
+        System.out.println("Default test() method running, consider providing your own");
+    }
+
+    /**
+     * Robot main program for free-form programs.
+     *
+     * This should be overridden by user subclasses if the intent is to not use the autonomous() and
+     * operatorControl() methods. In that case, the program is responsible for sensing when to run
+     * the autonomous and operator control functions in their program.
+     *
+     * This method will be called immediately after the constructor is called. If it has not been
+     * overridden by a user subclass (i.e. the default version runs), then the robotInit(), disabled(), autonomous() and
+     * operatorControl() methods will be called.
+     */
+    public void robotMain() {
+        m_robotMainOverridden = false;
+    }
+
+    /**
+     * Start a competition.
+     * This code tracks the order of the field starting to ensure that everything happens
+     * in the right order. Repeatedly run the correct method, either Autonomous or OperatorControl
+     * when the robot is enabled. After running the correct method, wait for some state to change,
+     * either the other mode starts or the robot is disabled. Then go back and wait for the robot
+     * to be enabled again.
+     */
+    public void startCompetition() {
+        // UsageReporting.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Simple);
+
+        robotMain();
+        if (!m_robotMainOverridden) {
+            // first and one-time initialization
+            LiveWindow.setEnabled(false);
+            robotInit();
+
+            while (true) {
+                if (isDisabled()) {
+                    // TODO: m_ds.InDisabled(true);
+                    disabled();
+                    // TODO: m_ds.InDisabled(false);
+                    while (isDisabled()) {
+                    	try {
+							Thread.sleep(10);
+						} catch (InterruptedException e) {}
+                    }
+                } else if (isAutonomous()) {
+                    // TODO: m_ds.InAutonomous(true);
+                    autonomous();
+                    // TODO: m_ds.InAutonomous(false);
+                    while (isAutonomous() && !isDisabled()) {
+                    	try {
+							Thread.sleep(10);
+						} catch (InterruptedException e) {}
+                    }
+                } else if (isTest()) {
+                    LiveWindow.setEnabled(true);
+                    // TODO: m_ds.InTest(true);
+                    test();
+                    // TODO: m_ds.InTest(false);
+                    while (isTest() && isEnabled()) {
+                    	try {
+							Thread.sleep(10);
+						} catch (InterruptedException e) {}
+                    }
+                    LiveWindow.setEnabled(false);
+                } else {
+                    // TODO: m_ds.InOperatorControl(true);
+                    operatorControl();
+                    // TODO: m_ds.InOperatorControl(false);
+                    while (isOperatorControl() && !isDisabled()) {
+                    	try {
+							Thread.sleep(10);
+						} catch (InterruptedException e) {}
+                    }
+                }
+            } /* while loop */
+        }
+    }
+}

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

@@ -0,0 +1,41 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Interface for speed controlling devices.
+ */
+public interface SpeedController extends PIDOutput {
+
+    /**
+     * Common interface for getting the current set speed of a speed controller.
+     *
+     * @return The current set speed.  Value is between -1.0 and 1.0.
+     */
+    double get();
+
+    /**
+     * Common interface for setting the speed of a speed controller.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
+     */
+    void set(double speed, byte syncGroup);
+
+    /**
+     * Common interface for setting the speed of a speed controller.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     */
+    void set(double speed);
+
+    /**
+     * Disable the speed controller
+     */
+    void disable();
+}

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

@@ -0,0 +1,209 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimSpeedController;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+
+/**
+ * CTRE Talon Speed Controller
+ */
+public class Talon implements SpeedController, PIDOutput, MotorSafety, LiveWindowSendable {
+	private int module, channel;
+	private SimSpeedController impl;
+
+    /**
+     * Common initialization code called by all constructors.
+     */
+    private void initTalon(final int slot, final int channel) {
+    	this.module = slot;
+    	this.channel = channel;
+    	impl = new SimSpeedController("simulator/pwm/"+slot+"/"+channel);
+    	
+        m_safetyHelper = new MotorSafetyHelper(this);
+        m_safetyHelper.setExpiration(0.0);
+        m_safetyHelper.setSafetyEnabled(false);
+        
+        // LiveWindow.addActuator("Talon", getModuleNumber(), getChannel(), this);
+        // UsageReporting.report(tResourceType.kResourceType_Talon, getChannel(), getModuleNumber()-1);
+    }
+
+    /**
+     * Constructor that assumes the default digital module.
+     *
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Talon(final int channel) {
+        initTalon(1, channel);
+    }
+
+    /**
+     * Constructor that specifies the digital module.
+     *
+     * @param slot The slot in the chassis that the digital module is plugged into.
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Talon(final int slot, final int channel) {
+        initTalon(slot, channel);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * @deprecated For compatibility with CANJaguar
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
+     */
+    public void set(double speed, byte syncGroup) {
+    	impl.set(speed, syncGroup);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed value between -1.0 and 1.0 to set.
+     */
+    public void set(double speed) {
+    	impl.set(speed);
+    }
+
+    /**
+     * Get the recently set value of the PWM.
+     *
+     * @return The most recently set value for the PWM between -1.0 and 1.0.
+     */
+    public double get() {
+        return impl.get();
+    }
+	
+    /**
+     * Disable the speed controller
+     */
+    public void disable() {
+		impl.set(0);
+	}
+
+    /**
+     * Write out the PID value as seen in the PIDOutput base object.
+     *
+     * @param output Write out the PWM value as was found in the PIDController
+     */
+    public void pidWrite(double output) {
+        impl.pidWrite(output);
+    }
+
+    //// MotorSafety Methods
+    private MotorSafetyHelper m_safetyHelper;
+    
+	@Override
+	public void setExpiration(double timeout) {
+        m_safetyHelper.setExpiration(timeout);
+	}
+
+	@Override
+	public double getExpiration() {
+        return m_safetyHelper.getExpiration();
+	}
+
+	@Override
+	public boolean isAlive() {
+        return m_safetyHelper.isAlive();
+	}
+
+	@Override
+	public void stopMotor() {
+        disable();
+	}
+
+	@Override
+	public void setSafetyEnabled(boolean enabled) {
+        m_safetyHelper.setSafetyEnabled(enabled);
+	}
+
+	@Override
+	public boolean isSafetyEnabled() {
+        return m_safetyHelper.isSafetyEnabled();
+	}
+
+	@Override
+	public String getDescription() {
+        return "PWM "+channel+" on module "+module;
+	}
+
+	//// LiveWindow Methods
+    private ITable m_table;
+    private ITableListener m_table_listener;
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void initTable(ITable subtable) {
+        m_table = subtable;
+        updateTable();
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public ITable getTable() {
+        return m_table;
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public String getSmartDashboardType() {
+        return "Speed Controller";
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void updateTable() {
+        if (m_table != null) {
+            m_table.putNumber("Value", get());
+        }
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void startLiveWindowMode() {
+        set(0.0); // Stop for safety
+        m_table_listener = new ITableListener() {
+            public void valueChanged(ITable itable, String key, Object value, boolean bln) {
+                set(((Double) value).doubleValue());
+            }
+        };
+        m_table.addTableListener("Value", m_table_listener, true);
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void stopLiveWindowMode() {
+        set(0); // Stop for safety
+        // TODO: Broken, should only remove the listener from "Value" only.
+        m_table.removeTableListener(m_table_listener);
+	}
+}

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

@@ -0,0 +1,126 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * Timer objects measure accumulated time in milliseconds.
+ * The timer object functions like a stopwatch. It can be started, stopped, and cleared. When the
+ * timer is running its value counts up in milliseconds. When stopped, the timer holds the current
+ * value. The implementation simply records the time when started and subtracts the current time
+ * whenever the value is requested.
+ */
+public class Timer {
+
+    private long m_startTime;
+    private double m_accumulatedTime;
+    private boolean m_running;
+
+    /**
+     * Pause the thread for a specified time. Pause the execution of the
+     * thread for a specified period of time given in seconds. Motors will
+     * continue to run at their last assigned values, and sensors will continue
+     * to update. Only the task containing the wait will pause until the wait
+     * time is expired.
+     *
+     * @param seconds Length of time to pause
+     */
+    public static void delay(final double seconds) {
+        try {
+            Thread.sleep((long) (seconds * 1e3));
+        } catch (final InterruptedException e) {
+        }
+    }
+
+    /**
+     * Return the system clock time in microseconds. Return the time from the
+     * FPGA hardware clock in microseconds since the FPGA started.
+     *
+     * @deprecated Use getFPGATimestamp instead.
+     * @return Robot running time in microseconds.
+     */
+    public static long getUsClock() {
+    	
+        return System.nanoTime() / 1000;
+    }
+
+    /**
+     * Return the system clock time in milliseconds. Return the time from the
+     * FPGA hardware clock in milliseconds since the FPGA started.
+     *
+     * @deprecated Use getFPGATimestamp instead.
+     * @return Robot running time in milliseconds.
+     */
+    static long getMsClock() {
+        return System.currentTimeMillis();
+    }
+
+    /**
+     * Return the system clock time in seconds. Return the time from the
+     * FPGA hardware clock in seconds since the FPGA started.
+     *
+     * @return Robot running time in seconds.
+     */
+    public static double getFPGATimestamp() {
+        return System.currentTimeMillis() / 1000.0;
+    }
+
+    /**
+     * Create a new timer object.
+     * Create a new timer object and reset the time to zero. The timer is initially not running and
+     * must be started.
+     */
+    public Timer() {
+        reset();
+    }
+
+    /**
+     * Get the current time from the timer. If the clock is running it is derived from
+     * the current system clock the start time stored in the timer class. If the clock
+     * is not running, then return the time when it was last stopped.
+     *
+     * @return Current time value for this timer in seconds
+     */
+    public synchronized double get() {
+        if (m_running) {
+            return ((double) ((getMsClock() - m_startTime) + m_accumulatedTime)) / 1000.0;
+        } else {
+            return m_accumulatedTime;
+        }
+    }
+
+    /**
+     * Reset the timer by setting the time to 0.
+     * Make the timer startTime the current time so new requests will be relative now
+     */
+    public synchronized void reset() {
+        m_accumulatedTime = 0;
+        m_startTime = getMsClock();
+    }
+
+    /**
+     * Start the timer running.
+     * Just set the running flag to true indicating that all time requests should be
+     * relative to the system clock.
+     */
+    public synchronized void start() {
+        m_startTime = getMsClock();
+        m_running = true;
+    }
+
+    /**
+     * Stop the timer.
+     * This computes the time as of now and clears the running flag, causing all
+     * subsequent time requests to be read from the accumulated time rather than
+     * looking at the system clock.
+     */
+    public synchronized void stop() {
+        final double temp = get();
+        m_accumulatedTime = temp;
+        m_running = false;
+    }
+}

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

@@ -0,0 +1,209 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.simulation.SimSpeedController;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+
+/**
+ * CTRE Talon Speed Controller
+ */
+public class Victor implements SpeedController, PIDOutput, MotorSafety, LiveWindowSendable {
+	private int module, channel;
+	private SimSpeedController impl;
+
+    /**
+     * Common initialization code called by all constructors.
+     */
+    private void initTalon(final int slot, final int channel) {
+    	this.module = slot;
+    	this.channel = channel;
+    	impl = new SimSpeedController("simulator/pwm/"+slot+"/"+channel);
+    	
+        m_safetyHelper = new MotorSafetyHelper(this);
+        m_safetyHelper.setExpiration(0.0);
+        m_safetyHelper.setSafetyEnabled(false);
+        
+        // LiveWindow.addActuator("Talon", getModuleNumber(), getChannel(), this);
+        // UsageReporting.report(tResourceType.kResourceType_Talon, getChannel(), getModuleNumber()-1);
+    }
+
+    /**
+     * Constructor that assumes the default digital module.
+     *
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Victor(final int channel) {
+        initTalon(1, channel);
+    }
+
+    /**
+     * Constructor that specifies the digital module.
+     *
+     * @param slot The slot in the chassis that the digital module is plugged into.
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public Victor(final int slot, final int channel) {
+        initTalon(slot, channel);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * @deprecated For compatibility with CANJaguar
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
+     */
+    public void set(double speed, byte syncGroup) {
+    	impl.set(speed, syncGroup);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed value between -1.0 and 1.0 to set.
+     */
+    public void set(double speed) {
+    	impl.set(speed);
+    }
+
+    /**
+     * Get the recently set value of the PWM.
+     *
+     * @return The most recently set value for the PWM between -1.0 and 1.0.
+     */
+    public double get() {
+        return impl.get();
+    }
+	
+    /**
+     * Disable the speed controller
+     */
+    public void disable() {
+		impl.set(0);
+	}
+
+    /**
+     * Write out the PID value as seen in the PIDOutput base object.
+     *
+     * @param output Write out the PWM value as was found in the PIDController
+     */
+    public void pidWrite(double output) {
+        impl.pidWrite(output);
+    }
+
+    //// MotorSafety Methods
+    private MotorSafetyHelper m_safetyHelper;
+    
+	@Override
+	public void setExpiration(double timeout) {
+        m_safetyHelper.setExpiration(timeout);
+	}
+
+	@Override
+	public double getExpiration() {
+        return m_safetyHelper.getExpiration();
+	}
+
+	@Override
+	public boolean isAlive() {
+        return m_safetyHelper.isAlive();
+	}
+
+	@Override
+	public void stopMotor() {
+        disable();
+	}
+
+	@Override
+	public void setSafetyEnabled(boolean enabled) {
+        m_safetyHelper.setSafetyEnabled(enabled);
+	}
+
+	@Override
+	public boolean isSafetyEnabled() {
+        return m_safetyHelper.isSafetyEnabled();
+	}
+
+	@Override
+	public String getDescription() {
+        return "PWM "+channel+" on module "+module;
+	}
+
+	//// LiveWindow Methods
+    private ITable m_table;
+    private ITableListener m_table_listener;
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void initTable(ITable subtable) {
+        m_table = subtable;
+        updateTable();
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public ITable getTable() {
+        return m_table;
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public String getSmartDashboardType() {
+        return "Speed Controller";
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void updateTable() {
+        if (m_table != null) {
+            m_table.putNumber("Value", get());
+        }
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void startLiveWindowMode() {
+        set(0.0); // Stop for safety
+        m_table_listener = new ITableListener() {
+            public void valueChanged(ITable itable, String key, Object value, boolean bln) {
+                set(((Double) value).doubleValue());
+            }
+        };
+        m_table.addTableListener("Value", m_table_listener, true);
+	}
+
+    /**
+     * {@inheritDoc}
+     */
+	@Override
+	public void stopLiveWindowMode() {
+        set(0); // Stop for safety
+        // TODO: Broken, should only remove the listener from "Value" only.
+        m_table.removeTableListener(m_table_listener);
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java

@@ -0,0 +1,70 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+/**
+ * This class provides an easy way to link commands to OI inputs.
+ *
+ * It is very easy to link a button to a command.  For instance, you could
+ * link the trigger button of a joystick to a "score" command.
+ *
+ * This class represents a subclass of Trigger that is specifically aimed at
+ * buttons on an operator interface as a common use case of the more generalized
+ * Trigger objects. This is a simple wrapper around Trigger with the method names
+ * renamed to fit the Button object use.
+ *
+ * @author brad
+ */
+public abstract class Button extends Trigger {
+
+    /**
+     * Starts the given command whenever the button is newly pressed.
+     * @param command the command to start
+     */
+    public void whenPressed(final Command command) {
+        whenActive(command);
+    }
+
+    /**
+     * Constantly starts the given command while the button is held.
+     *
+     * {@link Command#start()} will be called repeatedly while the button is held,
+     * and will be canceled when the button is released.
+     *
+     * @param command the command to start
+     */
+    public void whileHeld(final Command command) {
+        whileActive(command);
+    }
+
+    /**
+     * Starts the command when the button is released
+     * @param command the command to start
+     */
+    public void whenReleased(final Command command) {
+        whenInactive(command);
+    }
+
+    /**
+     * Toggles the command whenever the button is pressed (on then off then on)
+     * @param command
+     */
+    public void toggleWhenPressed(final Command command) {
+        toggleWhenActive(command);
+    }
+
+    /**
+     * Cancel the command when the button is pressed
+     * @param command
+     */
+    public void cancelWhenPressed(final Command command) {
+        cancelWhenActive(command);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java

@@ -0,0 +1,48 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+/**
+ * This class is intended to be used within a program.  The programmer can manually set its value.
+ * Also includes a setting for whether or not it should invert its value.
+ *
+ * @author Joe
+ */
+public class InternalButton extends Button {
+
+    boolean pressed;
+    boolean inverted;
+
+    /**
+     * Creates an InternalButton that is not inverted.
+     */
+    public InternalButton() {
+        this(false);
+    }
+
+    /**
+     * Creates an InternalButton which is inverted depending on the input.
+     *
+     * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed when set to false.
+     */
+    public InternalButton(boolean inverted) {
+        this.pressed = this.inverted = inverted;
+    }
+
+    public void setInverted(boolean inverted) {
+        this.inverted = inverted;
+    }
+
+    public void setPressed(boolean pressed) {
+        this.pressed = pressed;
+    }
+
+    public boolean get() {
+        return pressed ^ inverted;
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.GenericHID;
+
+/**
+ *
+ * @author bradmiller
+ */
+public class JoystickButton extends Button {
+
+    GenericHID m_joystick;
+    int m_buttonNumber;
+
+    /**
+     * Create a joystick button for triggering commands
+     * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
+     * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
+     */
+    public JoystickButton(GenericHID joystick, int buttonNumber) {
+        m_joystick = joystick;
+        m_buttonNumber = buttonNumber;
+    }
+
+    /**
+     * Gets the value of the joystick button
+     * @return The value of the joystick button
+     */
+    public boolean get() {
+        return m_joystick.getRawButton(m_buttonNumber);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+
+/**
+ *
+ * @author Joe
+ */
+public class NetworkButton extends Button {
+
+    NetworkTable table;
+    String field;
+
+    public NetworkButton(String table, String field) {
+        this(NetworkTable.getTable(table), field);
+    }
+
+    public NetworkButton(NetworkTable table, String field) {
+        this.table = table;
+        this.field = field;
+    }
+
+    public boolean get() {
+        return table.isConnected() && table.getBoolean(field, false);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java

@@ -0,0 +1,200 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.buttons;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * This class provides an easy way to link commands to inputs.
+ *
+ * It is very easy to link a button to a command.  For instance, you could
+ * link the trigger button of a joystick to a "score" command.
+ *
+ * It is encouraged that teams write a subclass of Trigger if they want to have
+ * something unusual (for instance, if they want to react to the user holding
+ * a button while the robot is reading a certain sensor input).  For this, they
+ * only have to write the {@link Trigger#get()} method to get the full functionality
+ * of the Trigger class.
+ *
+ * @author Joe Grinstead
+ */
+public abstract class Trigger implements Sendable {
+
+    /**
+     * Returns whether or not the trigger is active
+     *
+     * This method will be called repeatedly a command is linked to the Trigger.
+     *
+     * @return whether or not the trigger condition is active.
+     */
+    public abstract boolean get();
+
+    /**
+     * Returns whether get() return true or the internal table for SmartDashboard use is pressed.
+     * @return  whether get() return true or the internal table for SmartDashboard use is pressed
+     */
+    private boolean grab() {
+        return get() || (table != null /*&& table.isConnected()*/ && table.getBoolean("pressed", false));//FIXME make is connected work?
+    }
+
+    /**
+     * Starts the given command whenever the trigger just becomes active.
+     * @param command the command to start
+     */
+    public void whenActive(final Command command) {
+        new ButtonScheduler() {
+
+            boolean pressedLast = grab();
+
+            public void execute() {
+                if (grab()) {
+                    if (!pressedLast) {
+                        pressedLast = true;
+                        command.start();
+                    }
+                } else {
+                    pressedLast = false;
+                }
+            }
+        } .start();
+    }
+
+    /**
+     * Constantly starts the given command while the button is held.
+     *
+     * {@link Command#start()} will be called repeatedly while the trigger is active,
+     * and will be canceled when the trigger becomes inactive.
+     *
+     * @param command the command to start
+     */
+    public void whileActive(final Command command) {
+        new ButtonScheduler() {
+
+            boolean pressedLast = grab();
+
+            public void execute() {
+                if (grab()) {
+                    pressedLast = true;
+                    command.start();
+                } else {
+                    if (pressedLast) {
+                        pressedLast = false;
+                        command.cancel();
+                    }
+                }
+            }
+        } .start();
+    }
+
+    /**
+     * Starts the command when the trigger becomes inactive
+     * @param command the command to start
+     */
+    public void whenInactive(final Command command) {
+        new ButtonScheduler() {
+
+            boolean pressedLast = grab();
+
+            public void execute() {
+                if (grab()) {
+                    pressedLast = true;
+                } else {
+                    if (pressedLast) {
+                        pressedLast = false;
+                        command.start();
+                    }
+                }
+            }
+        } .start();
+    }
+
+    /**
+      * Toggles a command when the trigger becomes active
+      * @param command the command to toggle
+      */
+    public void toggleWhenActive(final Command command) {
+        new ButtonScheduler() {
+
+            boolean pressedLast = grab();
+
+            public void execute() {
+                if (grab()) {
+                    if (!pressedLast) {
+                        pressedLast = true;
+                        if (command.isRunning()) {
+                            command.cancel();
+                        } else {
+                            command.start();
+                        }
+                    }
+                } else {
+                    pressedLast = false;
+                }
+            }
+        } .start();
+    }
+
+    /**
+      * Cancels a command when the trigger becomes active
+      * @param command the command to cancel
+      */
+    public void cancelWhenActive(final Command command) {
+        new ButtonScheduler() {
+
+            boolean pressedLast = grab();
+
+            public void execute() {
+                if (grab()) {
+                    if (!pressedLast) {
+                        pressedLast = true;
+                        command.cancel();
+                    }
+                } else {
+                    pressedLast = false;
+                }
+            }
+        } .start();
+    }
+
+    /**
+     * An internal class of {@link Trigger}.  The user should ignore this, it is
+     * only public to interface between packages.
+     */
+    public abstract class ButtonScheduler {
+        public abstract void execute();
+
+        protected void start() {
+            Scheduler.getInstance().addButton(this);
+        }
+    }
+
+    /**
+     * These methods continue to return the "Button" SmartDashboard type until we decided
+     * to create a Trigger widget type for the dashboard.
+     */
+    public String getSmartDashboardType() {
+        return "Button";
+    }
+    private ITable table;
+    public void initTable(ITable table) {
+        this.table = table;
+        if(table!=null) {
+            table.putBoolean("pressed", get());
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return table;
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/Command.java

@@ -0,0 +1,522 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.ITableListener;
+import java.util.Enumeration;
+import java.util.NoSuchElementException;
+
+/**
+ * The Command class is at the very core of the entire command framework.
+ * Every command can be started with a call to {@link Command#start() start()}.
+ * Once a command is started it will call {@link Command#initialize() initialize()}, and then
+ * will repeatedly call {@link Command#execute() execute()} until the {@link Command#isFinished() isFinished()}
+ * returns true.  Once it does, {@link Command#end() end()} will be called.
+ *
+ * <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called, then
+ * the command will be stopped and {@link Command#interrupted() interrupted()} will be called.</p>
+ *
+ * <p>If a command uses a {@link Subsystem}, then it should specify that it does so by
+ * calling the {@link Command#requires(Subsystem) requires(...)} method
+ * in its constructor. Note that a Command may have multiple requirements, and
+ * {@link Command#requires(Subsystem) requires(...)} should be
+ * called for each one.</p>
+ *
+ * <p>If a command is running and a new command with shared requirements is started,
+ * then one of two things will happen.  If the active command is interruptible,
+ * then {@link Command#cancel() cancel()} will be called and the command will be removed
+ * to make way for the new one.  If the active command is not interruptible, the
+ * other one will not even be started, and the active one will continue functioning.</p>
+ *
+ * @author Brad Miller
+ * @author Joe Grinstead
+ * @see Subsystem
+ * @see CommandGroup
+ * @see IllegalUseOfCommandException
+ */
+public abstract class Command implements NamedSendable {
+
+    /** The name of this command */
+    private String m_name;
+    /** The time since this command was initialized */
+    private double m_startTime = -1;
+    /** The time (in seconds) before this command "times out" (or -1 if no timeout) */
+    private double m_timeout = -1;
+    /** Whether or not this command has been initialized */
+    private boolean m_initialized = false;
+    /** The requirements (or null if no requirements) */
+    private Set m_requirements;
+    /** Whether or not it is running */
+    private boolean m_running = false;
+    /** Whether or not it is interruptible*/
+    private boolean m_interruptible = true;
+    /** Whether or not it has been canceled */
+    private boolean m_canceled = false;
+    /** Whether or not it has been locked */
+    private boolean m_locked = false;
+    /** Whether this command should run when the robot is disabled */
+    private boolean m_runWhenDisabled = false;
+    /** The {@link CommandGroup} this is in */
+    private CommandGroup m_parent;
+
+    /**
+     * Creates a new command.
+     * The name of this command will be set to its class name.
+     */
+    public Command() {
+        m_name = getClass().getName();
+        m_name = m_name.substring(m_name.lastIndexOf('.') + 1);
+    }
+
+    /**
+     * Creates a new command with the given name.
+     * @param name the name for this command
+     * @throws IllegalArgumentException if name is null
+     */
+    public Command(String name) {
+        if (name == null) {
+            throw new IllegalArgumentException("Name must not be null.");
+        }
+        m_name = name;
+    }
+
+    /**
+     * Creates a new command with the given timeout and a default name.
+     * The default name is the name of the class.
+     * @param timeout the time (in seconds) before this command "times out"
+     * @throws IllegalArgumentException if given a negative timeout
+     * @see Command#isTimedOut() isTimedOut()
+     */
+    public Command(double timeout) {
+        this();
+        if (timeout < 0) {
+            throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
+        }
+        m_timeout = timeout;
+    }
+
+    /**
+     * Creates a new command with the given name and timeout.
+     * @param name the name of the command
+     * @param timeout the time (in seconds) before this command "times out"
+     * @throws IllegalArgumentException if given a negative timeout or name was null.
+     * @see Command#isTimedOut() isTimedOut()
+     */
+    public Command(String name, double timeout) {
+        this(name);
+        if (timeout < 0) {
+            throw new IllegalArgumentException("Timeout must not be negative.  Given:" + timeout);
+        }
+        m_timeout = timeout;
+    }
+
+    /**
+     * Returns the name of this command.
+     * If no name was specified in the constructor,
+     * then the default is the name of the class.
+     * @return the name of this command
+     */
+    public String getName() {
+        return m_name;
+    }
+
+    /**
+     * Sets the timeout of this command.
+     * @param seconds the timeout (in seconds)
+     * @throws IllegalArgumentException if seconds is negative
+     * @see Command#isTimedOut() isTimedOut()
+     */
+    protected synchronized final void setTimeout(double seconds) {
+        if (seconds < 0) {
+            throw new IllegalArgumentException("Seconds must be positive.  Given:" + seconds);
+        }
+        m_timeout = seconds;
+    }
+
+    /**
+     * Returns the time since this command was initialized (in seconds).
+     * This function will work even if there is no specified timeout.
+     * @return the time since this command was initialized (in seconds).
+     */
+    public synchronized final double timeSinceInitialized() {
+        return m_startTime < 0 ? 0 : Timer.getFPGATimestamp() - m_startTime;
+    }
+
+    /**
+     * This method specifies that the given {@link Subsystem} is used by this command.
+     * This method is crucial to the functioning of the Command System in general.
+     *
+     * <p>Note that the recommended way to call this method is in the constructor.</p>
+     *
+     * @param subsystem the {@link Subsystem} required
+     * @throws IllegalArgumentException if subsystem is null
+     * @throws IllegalUseOfCommandException if this command has started before or if it has been given to a {@link CommandGroup}
+     * @see Subsystem
+     */
+    protected synchronized void requires(Subsystem subsystem) {
+        validate("Can not add new requirement to command");
+        if (subsystem != null) {
+            if (m_requirements == null) {
+                m_requirements = new Set();
+            }
+            m_requirements.add(subsystem);
+        } else {
+            throw new IllegalArgumentException("Subsystem must not be null.");
+        }
+    }
+
+    /**
+     * Called when the command has been removed.
+     * This will call {@link Command#interrupted() interrupted()} or {@link Command#end() end()}.
+     */
+    synchronized void removed() {
+        if (m_initialized) {
+            if (isCanceled()) {
+                interrupted();
+                _interrupted();
+            } else {
+                end();
+                _end();
+            }
+        }
+        m_initialized = false;
+        m_canceled = false;
+        m_running = false;
+        if (table != null) {
+            table.putBoolean("running", false);
+        }
+    }
+
+    /**
+     * The run method is used internally to actually run the commands.
+     * @return whether or not the command should stay within the {@link Scheduler}.
+     */
+    synchronized boolean run() {
+        // XXX: if (!m_runWhenDisabled && m_parent == null && DriverStation.getInstance().isDisabled()) {
+    	// XXX:     cancel();
+        // XXX: }
+        if (isCanceled()) {
+            return false;
+        }
+        if (!m_initialized) {
+            m_initialized = true;
+            startTiming();
+            _initialize();
+            initialize();
+        }
+        _execute();
+        execute();
+        return !isFinished();
+    }
+
+    /**
+     * The initialize method is called the first time this Command is run after
+     * being started.
+     */
+    protected abstract void initialize();
+
+    /** A shadow method called before {@link Command#initialize() initialize()} */
+    void _initialize() {
+    }
+
+    /**
+     * The execute method is called repeatedly until this Command either finishes
+     * or is canceled.
+     */
+    protected abstract void execute();
+
+    /** A shadow method called before {@link Command#execute() execute()} */
+    void _execute() {
+    }
+
+    /**
+     * Returns whether this command is finished.
+     * If it is, then the command will be removed
+     * and {@link Command#end() end()} will be called.
+     *
+     * <p>It may be useful for a team to reference the {@link Command#isTimedOut() isTimedOut()} method
+     * for time-sensitive commands.</p>
+     * @return whether this command is finished.
+     * @see Command#isTimedOut() isTimedOut()
+     */
+    protected abstract boolean isFinished();
+
+    /**
+     * Called when the command ended peacefully.  This is where you may want
+     * to wrap up loose ends, like shutting off a motor that was being used
+     * in the command.
+     */
+    protected abstract void end();
+
+    /** A shadow method called after {@link Command#end() end()}. */
+    void _end() {
+    }
+
+    /**
+     * Called when the command ends because somebody called {@link Command#cancel() cancel()}
+     * or another command shared the same requirements as this one, and booted
+     * it out.
+     *
+     * <p>This is where you may want
+     * to wrap up loose ends, like shutting off a motor that was being used
+     * in the command.</p>
+     *
+     * <p>Generally, it is useful to simply call the {@link Command#end() end()} method
+     * within this method</p>
+     */
+    protected abstract void interrupted();
+
+    /** A shadow method called after {@link Command#interrupted() interrupted()}. */
+    void _interrupted() {
+    }
+
+    /**
+     * Called to indicate that the timer should start.
+     * This is called right before {@link Command#initialize() initialize()} is, inside the
+     * {@link Command#run() run()} method.
+     */
+    private void startTiming() {
+        m_startTime = Timer.getFPGATimestamp();
+    }
+
+    /**
+     * Returns whether or not the {@link Command#timeSinceInitialized() timeSinceInitialized()}
+     * method returns a number which is greater than or equal to the timeout for the command.
+     * If there is no timeout, this will always return false.
+     * @return whether the time has expired
+     */
+    protected synchronized boolean isTimedOut() {
+        return m_timeout != -1 && timeSinceInitialized() >= m_timeout;
+    }
+
+    /**
+     * Returns the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem Subsystems}) of this command
+     * @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem Subsystems}) of this command
+     */
+    synchronized Enumeration getRequirements() {
+        return m_requirements == null ? emptyEnumeration : m_requirements.getElements();
+    }
+
+    /**
+     * Prevents further changes from being made
+     */
+    synchronized void lockChanges() {
+        m_locked = true;
+    }
+
+    /**
+     * If changes are locked, then this will throw an {@link IllegalUseOfCommandException}.
+     * @param message the message to say (it is appended by a default message)
+     */
+    synchronized void validate(String message) {
+        if (m_locked) {
+            throw new IllegalUseOfCommandException(message + " after being started or being added to a command group");
+        }
+    }
+
+    /**
+     * Sets the parent of this command.  No actual change is made to the group.
+     * @param parent the parent
+     * @throws IllegalUseOfCommandException if this {@link Command} already is already in a group
+     */
+    synchronized void setParent(CommandGroup parent) {
+        if (this.m_parent != null) {
+            throw new IllegalUseOfCommandException("Can not give command to a command group after already being put in a command group");
+        }
+        lockChanges();
+        this.m_parent = parent;
+        if (table != null) {
+            table.putBoolean("isParented", true);
+        }
+    }
+
+    /**
+     * Starts up the command.  Gets the command ready to start.
+     * <p>Note that the command will eventually start, however it will not necessarily
+     * do so immediately, and may in fact be canceled before initialize is even called.</p>
+     * @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
+     */
+    public synchronized void start() {
+        lockChanges();
+        if (m_parent != null) {
+            throw new IllegalUseOfCommandException("Can not start a command that is a part of a command group");
+        }
+        Scheduler.getInstance().add(this);
+    }
+
+    /**
+     * This is used internally to mark that the command has been started.
+     * The lifecycle of a command is:
+     *
+     * startRunning() is called.
+     * run() is called (multiple times potentially)
+     * removed() is called
+     *
+     * It is very important that startRunning and removed be called in order or some assumptions
+     * of the code will be broken.
+     */
+    synchronized void startRunning() {
+        m_running = true;
+        m_startTime = -1;
+        if (table != null) {
+            table.putBoolean("running", true);
+        }
+    }
+
+    /**
+     * Returns whether or not the command is running.
+     * This may return true even if the command has just been canceled, as it may
+     * not have yet called {@link Command#interrupted()}.
+     * @return whether or not the command is running
+     */
+    public synchronized boolean isRunning() {
+        return m_running;
+    }
+
+    /**
+     * This will cancel the current command.
+     * <p>This will cancel the current command eventually.  It can be called multiple times.
+     * And it can be called when the command is not running.  If the command is running though,
+     * then the command will be marked as canceled and eventually removed.</p>
+     * <p>A command can not be canceled
+     * if it is a part of a command group, you must cancel the command group instead.</p>
+     * @throws IllegalUseOfCommandException if this command is a part of a command group
+     */
+    public synchronized void cancel() {
+        if (m_parent != null) {
+            throw new IllegalUseOfCommandException("Can not manually cancel a command in a command group");
+        }
+        _cancel();
+    }
+
+    /**
+     * This works like cancel(), except that it doesn't throw an exception if it is a part
+     * of a command group.  Should only be called by the parent command group.
+     */
+    synchronized void _cancel() {
+        if (isRunning()) {
+            m_canceled = true;
+        }
+    }
+
+    /**
+     * Returns whether or not this has been canceled.
+     * @return whether or not this has been canceled
+     */
+    public synchronized boolean isCanceled() {
+        return m_canceled;
+    }
+
+    /**
+     * Returns whether or not this command can be interrupted.
+     * @return whether or not this command can be interrupted
+     */
+    public synchronized boolean isInterruptible() {
+        return m_interruptible;
+    }
+
+    /**
+     * Sets whether or not this command can be interrupted.
+     * @param interruptible whether or not this command can be interrupted
+     */
+    protected synchronized void setInterruptible(boolean interruptible) {
+        this.m_interruptible = interruptible;
+    }
+
+    /**
+     * Checks if the command requires the given {@link Subsystem}.
+     * @param system the system
+     * @return whether or not the subsystem is required, or false if given null
+     */
+    public synchronized boolean doesRequire(Subsystem system) {
+        return m_requirements != null && m_requirements.contains(system);
+    }
+
+    /**
+     * Returns the {@link CommandGroup} that this command is a part of.
+     * Will return null if this {@link Command} is not in a group.
+     * @return the {@link CommandGroup} that this command is a part of (or null if not in group)
+     */
+    public synchronized CommandGroup getGroup() {
+        return m_parent;
+    }
+
+    /**
+     * Sets whether or not this {@link Command} should run when the robot is disabled.
+     *
+     * <p>By default a command will not run when the robot is disabled, and will in fact be canceled.</p>
+     * @param run whether or not this command should run when the robot is disabled
+     */
+    public void setRunWhenDisabled(boolean run) {
+        m_runWhenDisabled = run;
+    }
+
+    /**
+     * Returns whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself.
+     * @return whether or not this {@link Command} will run when the robot is disabled, or if it will cancel itself
+     */
+    public boolean willRunWhenDisabled() {
+        return m_runWhenDisabled;
+    }
+    /** An empty enumeration given whenever there are no requirements */
+    private static Enumeration emptyEnumeration = new Enumeration() {
+
+        public boolean hasMoreElements() {
+            return false;
+        }
+
+        public Object nextElement() {
+            throw new NoSuchElementException();
+        }
+    };
+
+    /**
+     * The string representation for a {@link Command} is by default its name.
+     * @return the string representation of this object
+     */
+    public String toString() {
+        return getName();
+    }
+
+
+    public String getSmartDashboardType() {
+        return "Command";
+    }
+    private ITableListener listener = new ITableListener() {
+        public void valueChanged(ITable table, String key, Object value, boolean isNew) {
+            if (((Boolean) value).booleanValue()) {
+                start();
+            } else {
+                cancel();
+            }
+        }
+    };
+    private ITable table;
+    public void initTable(ITable table) {
+        if(this.table!=null)
+            this.table.removeTableListener(listener);
+        this.table = table;
+        if(table!=null) {
+            table.putString("name", getName());
+            table.putBoolean("running", isRunning());
+            table.putBoolean("isParented", m_parent != null);
+            table.addTableListener("running", listener, false);
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return table;
+    }
+
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java

@@ -0,0 +1,398 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Vector;
+
+/**
+ * A {@link CommandGroup} is a list of commands which are executed in sequence.
+ *
+ * <p>Commands in a {@link CommandGroup} are added using the {@link CommandGroup#addSequential(Command) addSequential(...)} method
+ * and are called sequentially.
+ * {@link CommandGroup CommandGroups} are themselves {@link Command commands}
+ * and can be given to other {@link CommandGroup CommandGroups}.</p>
+ *
+ * <p>{@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command subcommands}.  Additional
+ * requirements can be specified by calling {@link CommandGroup#requires(Subsystem) requires(...)}
+ * normally in the constructor.</P>
+ *
+ * <p>CommandGroups can also execute commands in parallel, simply by adding them
+ * using {@link CommandGroup#addParallel(Command) addParallel(...)}.</p>
+ *
+ * @author Brad Miller
+ * @author Joe Grinstead
+ * @see Command
+ * @see Subsystem
+ * @see IllegalUseOfCommandException
+ */
+public class CommandGroup extends Command {
+
+    /** The commands in this group (stored in entries) */
+    private Vector m_commands = new Vector();
+    /** The active children in this group (stored in entries) */
+    Vector m_children = new Vector();
+    /** The current command, -1 signifies that none have been run */
+    private int m_currentCommandIndex = -1;
+
+    /**
+     * Creates a new {@link CommandGroup CommandGroup}.
+     * The name of this command will be set to its class name.
+     */
+    public CommandGroup() {
+    }
+
+    /**
+     * Creates a new {@link CommandGroup CommandGroup} with the given name.
+     * @param name the name for this command group
+     * @throws IllegalArgumentException if name is null
+     */
+    public CommandGroup(String name) {
+        super(name);
+    }
+
+    /**
+     * Adds a new {@link Command Command} to the group.  The {@link Command Command} will be started after
+     * all the previously added {@link Command Commands}.
+     *
+     * <p>Note that any requirements the given {@link Command Command} has will be added to the
+     * group.  For this reason, a {@link Command Command's} requirements can not be changed after
+     * being added to a group.</p>
+     *
+     * <p>It is recommended that this method be called in the constructor.</p>
+     *
+     * @param command The {@link Command Command} to be added
+     * @throws IllegalUseOfCommandException if the group has been started before or been given to another group
+     * @throws IllegalArgumentException if command is null
+     */
+    public synchronized final void addSequential(Command command) {
+        validate("Can not add new command to command group");
+        if (command == null) {
+            throw new IllegalArgumentException("Given null command");
+        }
+
+        command.setParent(this);
+
+        m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
+        for (Enumeration e = command.getRequirements(); e.hasMoreElements();) {
+            requires((Subsystem) e.nextElement());
+        }
+    }
+
+    /**
+     * Adds a new {@link Command Command} to the group with a given timeout.
+     * The {@link Command Command} will be started after all the previously added commands.
+     *
+     * <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
+     * expires, whichever is sooner.  Note that the given {@link Command Command} will have no
+     * knowledge that it is on a timer.</p>
+     *
+     * <p>Note that any requirements the given {@link Command Command} has will be added to the
+     * group.  For this reason, a {@link Command Command's} requirements can not be changed after
+     * being added to a group.</p>
+     *
+     * <p>It is recommended that this method be called in the constructor.</p>
+     *
+     * @param command The {@link Command Command} to be added
+     * @param timeout The timeout (in seconds)
+     * @throws IllegalUseOfCommandException if the group has been started before or been given to another group or
+     * if the {@link Command Command} has been started before or been given to another group
+     * @throws IllegalArgumentException if command is null or timeout is negative
+     */
+    public synchronized final void addSequential(Command command, double timeout) {
+        validate("Can not add new command to command group");
+        if (command == null) {
+            throw new IllegalArgumentException("Given null command");
+        }
+        if (timeout < 0) {
+            throw new IllegalArgumentException("Can not be given a negative timeout");
+        }
+
+        command.setParent(this);
+
+        m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
+        for (Enumeration e = command.getRequirements(); e.hasMoreElements();) {
+            requires((Subsystem) e.nextElement());
+        }
+    }
+
+    /**
+     * Adds a new child {@link Command} to the group.  The {@link Command} will be started after
+     * all the previously added {@link Command Commands}.
+     *
+     * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
+     * run at the same time as the subsequent {@link Command Commands}.  The child will run until either
+     * it finishes, a new child with conflicting requirements is started, or
+     * the main sequence runs a {@link Command} with conflicting requirements.  In the latter
+     * two cases, the child will be canceled even if it says it can't be
+     * interrupted.</p>
+     *
+     * <p>Note that any requirements the given {@link Command Command} has will be added to the
+     * group.  For this reason, a {@link Command Command's} requirements can not be changed after
+     * being added to a group.</p>
+     *
+     * <p>It is recommended that this method be called in the constructor.</p>
+     *
+     * @param command The command to be added
+     * @throws IllegalUseOfCommandException if the group has been started before or been given to another command group
+     * @throws IllegalArgumentException if command is null
+     */
+    public synchronized final void addParallel(Command command) {
+        validate("Can not add new command to command group");
+        if (command == null) {
+            throw new NullPointerException("Given null command");
+        }
+
+        command.setParent(this);
+
+        m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
+        for (Enumeration e = command.getRequirements(); e.hasMoreElements();) {
+            requires((Subsystem) e.nextElement());
+        }
+    }
+
+    /**
+     * Adds a new child {@link Command} to the group with the given timeout.  The {@link Command} will be started after
+     * all the previously added {@link Command Commands}.
+     *
+     * <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
+     * or the time expires, whichever is sooner.  Note that the given {@link Command Command} will have no
+     * knowledge that it is on a timer.</p>
+     *
+     * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it
+     * run at the same time as the subsequent {@link Command Commands}.  The child will run until either
+     * it finishes, the timeout expires, a new child with conflicting requirements is started, or
+     * the main sequence runs a {@link Command} with conflicting requirements.  In the latter
+     * two cases, the child will be canceled even if it says it can't be
+     * interrupted.</p>
+     *
+     * <p>Note that any requirements the given {@link Command Command} has will be added to the
+     * group.  For this reason, a {@link Command Command's} requirements can not be changed after
+     * being added to a group.</p>
+     *
+     * <p>It is recommended that this method be called in the constructor.</p>
+     *
+     * @param command The command to be added
+     * @param timeout The timeout (in seconds)
+     * @throws IllegalUseOfCommandException if the group has been started before or been given to another command group
+     * @throws IllegalArgumentException if command is null
+     */
+    public synchronized final void addParallel(Command command, double timeout) {
+        validate("Can not add new command to command group");
+        if (command == null) {
+            throw new NullPointerException("Given null command");
+        }
+        if (timeout < 0) {
+            throw new IllegalArgumentException("Can not be given a negative timeout");
+        }
+
+        command.setParent(this);
+
+        m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
+        for (Enumeration e = command.getRequirements(); e.hasMoreElements();) {
+            requires((Subsystem) e.nextElement());
+        }
+    }
+
+    void _initialize() {
+        m_currentCommandIndex = -1;
+    }
+
+    void _execute() {
+        Entry entry = null;
+        Command cmd = null;
+        boolean firstRun = false;
+        if (m_currentCommandIndex == -1) {
+            firstRun = true;
+            m_currentCommandIndex = 0;
+        }
+
+        while (m_currentCommandIndex < m_commands.size()) {
+
+            if (cmd != null) {
+                if (entry.isTimedOut()) {
+                    cmd._cancel();
+                }
+                if (cmd.run()) {
+                    break;
+                } else {
+                    cmd.removed();
+                    m_currentCommandIndex++;
+                    firstRun = true;
+                    cmd = null;
+                    continue;
+                }
+            }
+
+            entry = (Entry) m_commands.elementAt(m_currentCommandIndex);
+            cmd = null;
+
+            switch (entry.state) {
+            case Entry.IN_SEQUENCE:
+                cmd = entry.command;
+                if (firstRun) {
+                    cmd.startRunning();
+                    cancelConflicts(cmd);
+                }
+                firstRun = false;
+                break;
+            case Entry.BRANCH_PEER:
+                m_currentCommandIndex++;
+                entry.command.start();
+                break;
+            case Entry.BRANCH_CHILD:
+                m_currentCommandIndex++;
+                cancelConflicts(entry.command);
+                entry.command.startRunning();
+                m_children.addElement(entry);
+                break;
+            }
+        }
+
+        // Run Children
+        for (int i = 0; i < m_children.size(); i++) {
+            entry = (Entry) m_children.elementAt(i);
+            Command child = entry.command;
+            if (entry.isTimedOut()) {
+                child._cancel();
+            }
+            if (!child.run()) {
+                child.removed();
+                m_children.removeElementAt(i--);
+            }
+        }
+    }
+
+    void _end() {
+        // Theoretically, we don't have to check this, but we do if teams override the isFinished method
+        if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
+            Command cmd = ((Entry) m_commands.elementAt(m_currentCommandIndex)).command;
+            cmd._cancel();
+            cmd.removed();
+        }
+
+        Enumeration children = m_children.elements();
+        while (children.hasMoreElements()) {
+            Command cmd = ((Entry) children.nextElement()).command;
+            cmd._cancel();
+            cmd.removed();
+        }
+        m_children.removeAllElements();
+    }
+
+    void _interrupted() {
+        _end();
+    }
+
+    /**
+     * Returns true if all the {@link Command Commands} in this group
+     * have been started and have finished.
+     *
+     * <p>Teams may override this method, although they should probably
+     * reference super.isFinished() if they do.</p>
+     *
+     * @return whether this {@link CommandGroup} is finished
+     */
+    protected boolean isFinished() {
+        return m_currentCommandIndex >= m_commands.size() && m_children.isEmpty();
+    }
+
+    // Can be overwritten by teams
+    protected void initialize() {
+    }
+
+    // Can be overwritten by teams
+    protected void execute() {
+    }
+
+    // Can be overwritten by teams
+    protected void end() {
+    }
+
+    // Can be overwritten by teams
+    protected void interrupted() {
+    }
+
+    /**
+     * Returns whether or not this group is interruptible.
+     * A command group will be uninterruptible if {@link CommandGroup#setInterruptible(boolean) setInterruptable(false)}
+     * was called or if it is currently running an uninterruptible command
+     * or child.
+     *
+     * @return whether or not this {@link CommandGroup} is interruptible.
+     */
+    public synchronized boolean isInterruptible() {
+        if (!super.isInterruptible()) {
+            return false;
+        }
+
+        if (m_currentCommandIndex != -1 && m_currentCommandIndex < m_commands.size()) {
+            Command cmd = ((Entry) m_commands.elementAt(m_currentCommandIndex)).command;
+            if (!cmd.isInterruptible()) {
+                return false;
+            }
+        }
+
+        for (int i = 0; i < m_children.size(); i++) {
+            if (!((Entry) m_children.elementAt(i)).command.isInterruptible()) {
+                return false;
+            }
+        }
+
+        return true;
+    }
+
+    private void cancelConflicts(Command command) {
+        for (int i = 0; i < m_children.size(); i++) {
+            Command child = ((Entry) m_children.elementAt(i)).command;
+
+            Enumeration requirements = command.getRequirements();
+
+            while (requirements.hasMoreElements()) {
+                Object requirement = requirements.nextElement();
+                if (child.doesRequire((Subsystem) requirement)) {
+                    child._cancel();
+                    child.removed();
+                    m_children.removeElementAt(i--);
+                    break;
+                }
+            }
+        }
+    }
+
+    private static class Entry {
+
+        private static final int IN_SEQUENCE = 0;
+        private static final int BRANCH_PEER = 1;
+        private static final int BRANCH_CHILD = 2;
+        Command command;
+        int state;
+        double timeout;
+
+        Entry(Command command, int state) {
+            this.command = command;
+            this.state = state;
+            this.timeout = -1;
+        }
+
+        Entry(Command command, int state, double timeout) {
+            this.command = command;
+            this.state = state;
+            this.timeout = timeout;
+        }
+
+        boolean isTimedOut() {
+            if (timeout == -1) {
+                return false;
+            } else {
+                double time = command.timeSinceInitialized();
+                return time == 0 ? false : time >= timeout;
+            }
+        }
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * This exception will be thrown if a command is used illegally.  There are
+ * several ways for this to happen.
+ *
+ * <p>Basically, a command becomes "locked" after it is first started or added
+ * to a command group.</p>
+ *
+ * <p>This exception should be thrown if (after a command has been locked) its requirements
+ * change, it is put into multiple command groups,
+ * it is started from outside its command group, or it adds a new child.</p>
+ *
+ * @author Joe Grinstead
+ */
+public class IllegalUseOfCommandException extends RuntimeException {
+
+    /**
+     * Instantiates an {@link IllegalUseOfCommandException}.
+     */
+    public IllegalUseOfCommandException() {
+    }
+
+    /**
+     * Instantiates an {@link IllegalUseOfCommandException} with the given message.
+     * @param message the message
+     */
+    public IllegalUseOfCommandException(String message) {
+        super(message);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ *
+ * @author Greg
+ */
+class LinkedListElement {
+    private LinkedListElement next;
+    private LinkedListElement previous;
+    private Command data;
+
+    public LinkedListElement() {
+    }
+
+    public void setData(Command newData) {
+        data = newData;
+    }
+
+    public Command getData() {
+        return data;
+    }
+
+    public LinkedListElement getNext() {
+        return next;
+    }
+
+    public LinkedListElement getPrevious() {
+        return previous;
+    }
+
+    public void add(LinkedListElement l) {
+        if(next == null) {
+            next = l;
+            next.previous = this;
+        } else {
+            next.previous = l;
+            l.next = next;
+            l.previous = this;
+            next = l;
+        }
+    }
+
+    public LinkedListElement remove() {
+        if(previous == null && next == null) {
+
+        } else if(next == null) {
+            previous.next = null;
+        } else if(previous == null) {
+            next.previous = null;
+        } else {
+            next.previous = previous;
+            previous.next = next;
+        }
+        LinkedListElement n = next;
+        next = null;
+        previous = null;
+        return n;
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java

@@ -0,0 +1,191 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * This class defines a {@link Command} which interacts heavily with a PID loop.
+ *
+ * <p>It provides some convenience methods to run an internal {@link PIDController}.
+ * It will also start and stop said {@link PIDController} when the {@link PIDCommand} is
+ * first initialized and ended/interrupted.</p>
+ *
+ * @author Joe Grinstead
+ */
+public abstract class PIDCommand extends Command implements Sendable {
+
+    /** The internal {@link PIDController} */
+    private PIDController controller;
+    /** An output which calls {@link PIDCommand#usePIDOutput(double)} */
+    private PIDOutput output = new PIDOutput() {
+
+        public void pidWrite(double output) {
+            usePIDOutput(output);
+        }
+    };
+    /** A source which calls {@link PIDCommand#returnPIDInput()} */
+    private PIDSource source = new PIDSource() {
+
+        public double pidGet() {
+            return returnPIDInput();
+        }
+    };
+
+    /**
+     * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+     * @param name the name of the command
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     */
+    public PIDCommand(String name, double p, double i, double d) {
+        super(name);
+        controller = new PIDController(p, i, d, source, output);
+    }
+
+    /**
+     * Instantiates a {@link PIDCommand} that will use the given p, i and d values.  It will also space the time
+     * between PID loop calculations to be equal to the given period.
+     * @param name the name
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param period the time (in seconds) between calculations
+     */
+    public PIDCommand(String name, double p, double i, double d, double period) {
+        super(name);
+        controller = new PIDController(p, i, d, source, output, period);
+    }
+
+    /**
+     * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+     * It will use the class name as its name.
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     */
+    public PIDCommand(double p, double i, double d) {
+        controller = new PIDController(p, i, d, source, output);
+    }
+
+    /**
+     * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
+     * It will use the class name as its name..
+     * It will also space the time
+     * between PID loop calculations to be equal to the given period.
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param period the time (in seconds) between calculations
+     */
+    public PIDCommand(double p, double i, double d, double period) {
+        controller = new PIDController(p, i, d, source, output, period);
+    }
+
+    /**
+     * Returns the {@link PIDController} used by this {@link PIDCommand}.
+     * Use this if you would like to fine tune the pid loop.
+     *
+     * <p>Notice that calling {@link PIDController#setSetpoint(double) setSetpoint(...)} on the controller
+     * will not result in the setpoint being trimmed to be in
+     * the range defined by {@link PIDCommand#setSetpointRange(double, double) setSetpointRange(...)}.</p>
+     *
+     * @return the {@link PIDController} used by this {@link PIDCommand}
+     */
+    protected PIDController getPIDController() {
+        return controller;
+    }
+
+    void _initialize() {
+        controller.enable();
+    }
+
+    void _end() {
+        controller.disable();
+    }
+
+    void _interrupted() {
+        _end();
+    }
+
+    /**
+     * Adds the given value to the setpoint.
+     * If {@link PIDCommand#setRange(double, double) setRange(...)} was used,
+     * then the bounds will still be honored by this method.
+     * @param deltaSetpoint the change in the setpoint
+     */
+    public void setSetpointRelative(double deltaSetpoint) {
+        setSetpoint(getSetpoint() + deltaSetpoint);
+    }
+
+    /**
+     * Sets the setpoint to the given value.  If {@link PIDCommand#setRange(double, double) setRange(...)}
+     * was called,
+     * then the given setpoint
+     * will be trimmed to fit within the range.
+     * @param setpoint the new setpoint
+     */
+    protected void setSetpoint(double setpoint) {
+        controller.setSetpoint(setpoint);
+    }
+
+    /**
+     * Returns the setpoint.
+     * @return the setpoint
+     */
+    protected double getSetpoint() {
+        return controller.getSetpoint();
+    }
+
+    /**
+     * Returns the current position
+     * @return the current position
+     */
+    protected double getPosition() {
+        return returnPIDInput();
+    }
+
+    /**
+     * Returns the input for the pid loop.
+     *
+     * <p>It returns the input for the pid loop, so if this command was based
+     * off of a gyro, then it should return the angle of the gyro</p>
+     *
+     * <p>All subclasses of {@link PIDCommand} must override this method.</p>
+     *
+     * <p>This method will be called in a different thread then the {@link Scheduler} thread.</p>
+     *
+     * @return the value the pid loop should use as input
+     */
+    protected abstract double returnPIDInput();
+
+    /**
+     * Uses the value that the pid loop calculated.  The calculated value is the "output" parameter.
+     * This method is a good time to set motor values, maybe something along the lines of <code>driveline.tankDrive(output, -output)</code>
+     *
+     * <p>All subclasses of {@link PIDCommand} must override this method.</p>
+     *
+     * <p>This method will be called in a different thread then the {@link Scheduler} thread.</p>
+     *
+     * @param output the value the pid loop calculated
+     */
+    protected abstract void usePIDOutput(double output);
+
+    public String getSmartDashboardType() {
+        return "PIDCommand";
+    }
+    public void initTable(ITable table) {
+        controller.initTable(table);
+        super.initTable(table);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java

@@ -0,0 +1,260 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * This class is designed to handle the case where there is a {@link Subsystem}
+ * which uses a single {@link PIDController} almost constantly (for instance,
+ * an elevator which attempts to stay at a constant height).
+ *
+ * <p>It provides some convenience methods to run an internal {@link PIDController}.
+ * It also allows access to the internal {@link PIDController} in order to give total control
+ * to the programmer.</p>
+ *
+ * @author Joe Grinstead
+ */
+public abstract class PIDSubsystem extends Subsystem implements Sendable {
+
+    /** The internal {@link PIDController} */
+    private PIDController controller;
+    /** An output which calls {@link PIDCommand#usePIDOutput(double)} */
+    private PIDOutput output = new PIDOutput() {
+
+        public void pidWrite(double output) {
+            usePIDOutput(output);
+        }
+    };
+    /** A source which calls {@link PIDCommand#returnPIDInput()} */
+    private PIDSource source = new PIDSource() {
+
+        public double pidGet() {
+            return returnPIDInput();
+        }
+    };
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+     * @param name the name
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     */
+    public PIDSubsystem(String name, double p, double i, double d) {
+        super(name);
+        controller = new PIDController(p, i, d, source, output);
+    }
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+     * @param name the name
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param f the feed forward value
+     */
+    public PIDSubsystem(String name, double p, double i, double d, double f) {
+        super(name);
+        controller = new PIDController(p, i, d, f, source, output);
+    }
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.  It will also space the time
+     * between PID loop calculations to be equal to the given period.
+     * @param name the name
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param period the time (in seconds) between calculations
+     */
+    public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
+        super(name);
+        controller = new PIDController(p, i, d, f, source, output, period);
+    }
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+     * It will use the class name as its name.
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     */
+    public PIDSubsystem(double p, double i, double d) {
+        controller = new PIDController(p, i, d, source, output);
+    }
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+     * It will use the class name as its name.
+     * It will also space the time
+     * between PID loop calculations to be equal to the given period.
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param f the feed forward coefficient
+     * @param period the time (in seconds) between calculations
+     */
+    public PIDSubsystem(double p, double i, double d, double period, double f) {
+        controller = new PIDController(p, i, d, f, source, output, period);
+    }
+
+    /**
+     * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
+     * It will use the class name as its name.
+     * It will also space the time
+     * between PID loop calculations to be equal to the given period.
+     * @param p the proportional value
+     * @param i the integral value
+     * @param d the derivative value
+     * @param period the time (in seconds) between calculations
+     */
+    public PIDSubsystem(double p, double i, double d, double period) {
+        controller = new PIDController(p, i, d, source, output, period);
+    }
+
+    /**
+     * Returns the {@link PIDController} used by this {@link PIDSubsystem}.
+     * Use this if you would like to fine tune the pid loop.
+     *
+     * <p>Notice that calling {@link PIDController#setSetpoint(double) setSetpoint(...)} on the controller
+     * will not result in the setpoint being trimmed to be in
+     * the range defined by {@link PIDSubsystem#setSetpointRange(double, double) setSetpointRange(...)}.</p>
+     *
+     * @return the {@link PIDController} used by this {@link PIDSubsystem}
+     */
+    public PIDController getPIDController() {
+        return controller;
+    }
+
+
+    /**
+     * Adds the given value to the setpoint.
+     * If {@link PIDCommand#setRange(double, double) setRange(...)} was used,
+     * then the bounds will still be honored by this method.
+     * @param deltaSetpoint the change in the setpoint
+     */
+    public void setSetpointRelative(double deltaSetpoint) {
+        setSetpoint(getPosition() + deltaSetpoint);
+    }
+
+    /**
+     * Sets the setpoint to the given value.  If {@link PIDCommand#setRange(double, double) setRange(...)}
+     * was called,
+     * then the given setpoint
+     * will be trimmed to fit within the range.
+     * @param setpoint the new setpoint
+     */
+    public void setSetpoint(double setpoint) {
+        controller.setSetpoint(setpoint);
+    }
+
+    /**
+     * Returns the setpoint.
+     * @return the setpoint
+     */
+    public double getSetpoint() {
+        return controller.getSetpoint();
+    }
+
+    /**
+     * Returns the current position
+     * @return the current position
+     */
+    public double getPosition() {
+        return returnPIDInput();
+    }
+
+    /**
+     * Sets the maximum and minimum values expected from the input.
+     *
+     * @param minimumInput the minimum value expected from the input
+     * @param maximumInput the maximum value expected from the output
+     */
+    public void setInputRange(double minimumInput, double maximumInput) {
+        controller.setInputRange(minimumInput, maximumInput);
+    }
+
+    /**
+    * Set the absolute error which is considered tolerable for use with
+    * OnTarget. The value is in the same range as the PIDInput values.
+    * @param t A PIDController.Tolerance object instance that is for example
+    * AbsoluteTolerance or PercentageTolerance. E.g. setTolerance(new PIDController.AbsoluteTolerance(0.1))
+    */
+    public void setAbsoluteTolerance(double t) {
+        controller.setAbsoluteTolerance(t);
+    }
+
+    /**
+    * Set the percentage error which is considered tolerable for use with
+    * OnTarget. (Value of 15.0 == 15 percent)
+    * @param t A PIDController.Tolerance object instance that is for example
+    * AbsoluteTolerance or PercentageTolerance. E.g. setTolerance(new PIDController.AbsoluteTolerance(0.1))
+    */
+    public void setPercentTolerance(double p) {
+        controller.setPercentTolerance(p);
+    }
+
+    /**
+     * Return true if the error is within the percentage of the total input range,
+     * determined by setTolerance. This assumes that the maximum and minimum input
+     * were set using setInput.
+     * @return true if the error is less than the tolerance
+     */
+    public boolean onTarget() {
+        return controller.onTarget();
+    }
+
+    /**
+     * Returns the input for the pid loop.
+     *
+     * <p>It returns the input for the pid loop, so if this Subsystem was based
+     * off of a gyro, then it should return the angle of the gyro</p>
+     *
+     * <p>All subclasses of {@link PIDSubsystem} must override this method.</p>
+     *
+     * @return the value the pid loop should use as input
+     */
+    protected abstract double returnPIDInput();
+
+    /**
+     * Uses the value that the pid loop calculated.  The calculated value is the "output" parameter.
+     * This method is a good time to set motor values, maybe something along the lines of <code>driveline.tankDrive(output, -output)</code>
+     *
+     * <p>All subclasses of {@link PIDSubsystem} must override this method.</p>
+     *
+     * @param output the value the pid loop calculated
+     */
+    protected abstract void usePIDOutput(double output);
+
+    /**
+     * Enables the internal {@link PIDController}
+     */
+    public void enable() {
+        controller.enable();
+    }
+
+    /**
+     * Disables the internal {@link PIDController}
+     */
+    public void disable() {
+        controller.disable();
+    }
+
+    public String getSmartDashboardType() {
+        return "PIDSubsystem";
+    }
+    public void initTable(ITable table) {
+        controller.initTable(table);
+        super.initTable(table);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java

@@ -0,0 +1,46 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link PrintCommand} is a command which prints out a string when it is initialized, and then immediately finishes.
+ * It is useful if you want a {@link CommandGroup} to print out a string when it reaches a certain point.
+ *
+ * @author Joe Grinstead
+ */
+public class PrintCommand extends Command {
+
+    /** The message to print out */
+    private String message;
+
+    /**
+     * Instantiates a {@link PrintCommand} which will print the given message when it is run.
+     * @param message the message to print
+     */
+    public PrintCommand(String message) {
+        super("Print(\"" + message + "\"");
+        this.message = message;
+    }
+
+    protected void initialize() {
+        System.out.println(message);
+    }
+
+    protected void execute() {
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java

@@ -0,0 +1,368 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Hashtable;
+import java.util.Vector;
+
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+import edu.wpi.first.wpilibj.networktables2.type.NumberArray;
+import edu.wpi.first.wpilibj.networktables2.type.StringArray;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * The {@link Scheduler} is a singleton which holds the top-level running
+ * commands. It is in charge of both calling the command's
+ * {@link Command#run() run()} method and to make sure that there are no two
+ * commands with conflicting requirements running.
+ *
+ * <p>It is fine if teams wish to take control of the {@link Scheduler}
+ * themselves, all that needs to be done is to call
+ * {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link Scheduler#run() run()}
+ * often to have {@link Command Commands} function correctly. However, this is
+ * already done for you if you use the CommandBased Robot template.</p>
+ *
+ * @author Joe Grinstead
+ * @see Command
+ */
+public class Scheduler implements NamedSendable {
+
+    /**
+     * The Singleton Instance
+     */
+    private static Scheduler instance;
+
+    /**
+     * Returns the {@link Scheduler}, creating it if one does not exist.
+     *
+     * @return the {@link Scheduler}
+     */
+    public synchronized static Scheduler getInstance() {
+        return instance == null ? instance = new Scheduler() : instance;
+    }
+    /**
+     * A hashtable of active {@link Command Commands} to their
+     * {@link LinkedListElement}
+     */
+    private Hashtable commandTable = new Hashtable();
+    /**
+     * The {@link Set} of all {@link Subsystem Subsystems}
+     */
+    private Set subsystems = new Set();
+    /**
+     * The first {@link Command} in the list
+     */
+    private LinkedListElement firstCommand;
+    /**
+     * The last {@link Command} in the list
+     */
+    private LinkedListElement lastCommand;
+    /**
+     * Whether or not we are currently adding a command
+     */
+    private boolean adding = false;
+    /**
+     * Whether or not we are currently disabled
+     */
+    private boolean disabled = false;
+    /**
+     * A list of all {@link Command Commands} which need to be added
+     */
+    private Vector additions = new Vector();
+    private ITable m_table;
+    /**
+     * A list of all
+     * {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It
+     * is created lazily.
+     */
+    private Vector buttons;
+    private boolean m_runningCommandsChanged;
+
+    /**
+     * Instantiates a {@link Scheduler}.
+     */
+    private Scheduler() {
+        // UsageReporting.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
+    }
+
+    /**
+     * Adds the command to the {@link Scheduler}. This will not add the
+     * {@link Command} immediately, but will instead wait for the proper time in
+     * the {@link Scheduler#run()} loop before doing so. The command returns
+     * immediately and does nothing if given null.
+     *
+     * <p>Adding a {@link Command} to the {@link Scheduler} involves the
+     * {@link Scheduler} removing any {@link Command} which has shared
+     * requirements.</p>
+     *
+     * @param command the command to add
+     */
+    public void add(Command command) {
+        if (command != null) {
+            additions.addElement(command);
+        }
+    }
+
+    /**
+     * Adds a button to the {@link Scheduler}. The {@link Scheduler} will poll
+     * the button during its {@link Scheduler#run()}.
+     *
+     * @param button the button to add
+     */
+    public void addButton(ButtonScheduler button) {
+        if (buttons == null) {
+            buttons = new Vector();
+        }
+        buttons.addElement(button);
+    }
+
+    /**
+     * Adds a command immediately to the {@link Scheduler}. This should only be
+     * called in the {@link Scheduler#run()} loop. Any command with conflicting
+     * requirements will be removed, unless it is uninterruptable. Giving
+     * <code>null</code> does nothing.
+     *
+     * @param command the {@link Command} to add
+     */
+    private void _add(Command command) {
+        if (command == null) {
+            return;
+        }
+
+        // Check to make sure no adding during adding
+        if (adding) {
+            System.err.println("WARNING: Can not start command from cancel method.  Ignoring:" + command);
+            return;
+        }
+
+        // Only add if not already in
+        if (!commandTable.containsKey(command)) {
+
+            // Check that the requirements can be had
+            Enumeration requirements = command.getRequirements();
+            while (requirements.hasMoreElements()) {
+                Subsystem lock = (Subsystem) requirements.nextElement();
+                if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
+                    return;
+                }
+            }
+
+            // Give it the requirements
+            adding = true;
+            requirements = command.getRequirements();
+            while (requirements.hasMoreElements()) {
+                Subsystem lock = (Subsystem) requirements.nextElement();
+                if (lock.getCurrentCommand() != null) {
+                    lock.getCurrentCommand().cancel();
+                    remove(lock.getCurrentCommand());
+                }
+                lock.setCurrentCommand(command);
+            }
+            adding = false;
+
+            // Add it to the list
+            LinkedListElement element = new LinkedListElement();
+            element.setData(command);
+            if (firstCommand == null) {
+                firstCommand = lastCommand = element;
+            } else {
+                lastCommand.add(element);
+                lastCommand = element;
+            }
+            commandTable.put(command, element);
+
+            m_runningCommandsChanged = true;
+
+            command.startRunning();
+        }
+    }
+
+    /**
+     * Runs a single iteration of the loop. This method should be called often
+     * in order to have a functioning {@link Command} system. The loop has five
+     * stages:
+     *
+     * <ol> <li> Poll the Buttons </li> <li> Execute/Remove the Commands </li>
+     * <li> Send values to SmartDashboard </li> <li> Add Commands </li> <li> Add
+     * Defaults </li> </ol>
+     */
+    public void run() {
+
+        m_runningCommandsChanged = false;
+
+        if (disabled) {
+            return;
+        } // Don't run when disabled
+
+        // Get button input (going backwards preserves button priority)
+        if (buttons != null) {
+            for (int i = buttons.size() - 1; i >= 0; i--) {
+                ((ButtonScheduler) buttons.elementAt(i)).execute();
+            }
+        }
+        // Loop through the commands
+        LinkedListElement e = firstCommand;
+        while (e != null) {
+            Command c = e.getData();
+            e = e.getNext();
+            if (!c.run()) {
+                remove(c);
+                m_runningCommandsChanged = true;
+            }
+        }
+
+        // Add the new things
+        for (int i = 0; i < additions.size(); i++) {
+            _add((Command) additions.elementAt(i));
+        }
+        additions.removeAllElements();
+
+        // Add in the defaults
+        Enumeration locks = subsystems.getElements();
+        while (locks.hasMoreElements()) {
+            Subsystem lock = (Subsystem) locks.nextElement();
+            if (lock.getCurrentCommand() == null) {
+                _add(lock.getDefaultCommand());
+            }
+            lock.confirmCommand();
+        }
+
+        updateTable();
+    }
+
+    /**
+     * Registers a {@link Subsystem} to this {@link Scheduler}, so that the
+     * {@link Scheduler} might know if a default {@link Command} needs to be
+     * run. All {@link Subsystem Subsystems} should call this.
+     *
+     * @param system the system
+     */
+    void registerSubsystem(Subsystem system) {
+        if (system != null) {
+            subsystems.add(system);
+        }
+    }
+
+    /**
+     * Removes the {@link Command} from the {@link Scheduler}.
+     *
+     * @param command the command to remove
+     */
+    void remove(Command command) {
+        if (command == null || !commandTable.containsKey(command)) {
+            return;
+        }
+        LinkedListElement e = (LinkedListElement) commandTable.get(command);
+        commandTable.remove(command);
+
+        if (e.equals(lastCommand)) {
+            lastCommand = e.getPrevious();
+        }
+        if (e.equals(firstCommand)) {
+            firstCommand = e.getNext();
+        }
+        e.remove();
+
+        Enumeration requirements = command.getRequirements();
+        while (requirements.hasMoreElements()) {
+            ((Subsystem) requirements.nextElement()).setCurrentCommand(null);
+        }
+
+        command.removed();
+    }
+
+    /**
+     * Removes all commands
+     */
+    public void removeAll() {
+        // TODO: Confirm that this works with "uninteruptible" commands
+        while (firstCommand != null) {
+            remove(firstCommand.getData());
+        }
+    }
+
+    /**
+     * Disable the command scheduler.
+     */
+    public void disable() {
+        disabled = true;
+    }
+
+    /**
+     * Enable the command scheduler.
+     */
+    public void enable() {
+        disabled = false;
+    }
+
+    public String getName() {
+        return "Scheduler";
+    }
+
+    public String getType() {
+        return "Scheduler";
+    }
+    private StringArray commands;
+    private NumberArray ids, toCancel;
+
+    /**
+     * {@inheritDoc}
+     */
+    public void initTable(ITable subtable) {
+        m_table = subtable;
+        commands = new StringArray();
+        ids = new NumberArray();
+        toCancel = new NumberArray();
+
+        m_table.putValue("Names", commands);
+        m_table.putValue("Ids", ids);
+        m_table.putValue("Cancel", toCancel);
+    }
+
+    private void updateTable() {
+        if (m_table != null) {
+            // Get the commands to cancel
+            m_table.retrieveValue("Cancel", toCancel);
+            if (toCancel.size() > 0) {
+                for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
+                    for (int i = 0; i < toCancel.size(); i++) {
+                        if (e.getData().hashCode() == toCancel.get(i)) {
+                            e.getData().cancel();
+                        }
+                    }
+                }
+                toCancel.setSize(0);
+                m_table.putValue("Cancel", toCancel);
+            }
+
+            if (m_runningCommandsChanged) {
+                commands.setSize(0);
+                ids.setSize(0);
+                // Set the the running commands
+                for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
+                    commands.add(e.getData().getName());
+                    ids.add(e.getData().hashCode());
+                }
+                m_table.putValue("Names", commands);
+                m_table.putValue("Ids", ids);
+            }
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return m_table;
+    }
+
+    public String getSmartDashboardType() {
+        return "Scheduler";
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/Set.java

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import java.util.Enumeration;
+import java.util.Vector;
+
+/**
+ *
+ * @author Greg
+ */
+class Set {
+    Vector set = new Vector();
+
+    public Set() {
+    }
+
+    public void add(Object o) {
+        if(set.contains(o)) return;
+        set.addElement(o);
+    }
+
+    public void add(Set s) {
+        Enumeration stuff = s.getElements();
+        for(Enumeration e = stuff; e.hasMoreElements();) {
+            add(e.nextElement());
+        }
+    }
+
+    public boolean contains(Object o) {
+        return set.contains(o);
+    }
+
+    public Enumeration getElements() {
+        return set.elements();
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java

@@ -0,0 +1,47 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link StartCommand} will call the {@link Command#start() start()} method of another command when it is initialized
+ * and will finish immediately.
+ *
+ * @author Joe Grinstead
+ */
+public class StartCommand extends Command {
+
+    /** The command to fork */
+    private Command m_commandToFork;
+
+    /**
+     * Instantiates a {@link StartCommand} which will start the
+     * given command whenever its {@link Command#initialize() initialize()} is called.
+     * @param commandToStart the {@link Command} to start
+     */
+    public StartCommand(Command commandToStart) {
+        super("Start(" + commandToStart + ")");
+        m_commandToFork = commandToStart;
+    }
+
+    protected void initialize() {
+        m_commandToFork.start();
+    }
+
+    protected void execute() {
+    }
+
+    protected boolean isFinished() {
+        return true;
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java

@@ -0,0 +1,197 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+import java.util.Enumeration;
+import java.util.Vector;
+
+/**
+ * This class defines a major component of the robot.
+ *
+ * <p>A good example of a subsystem is the driveline, or a claw if the robot has one.</p>
+ *
+ * <p>All motors should be a part of a subsystem. For instance, all the wheel motors should be
+ * a part of some kind of "Driveline" subsystem.</p>
+ *
+ * <p>Subsystems are used within the command system as requirements for {@link Command}.
+ * Only one command which requires a subsystem can run at a time.  Also, subsystems
+ * can have default commands which are started if there is no command running which
+ * requires this subsystem.</p>
+ *
+ * @author Joe Grinstead
+ * @see Command
+ */
+public abstract class Subsystem implements NamedSendable {
+
+    /** Whether or not getDefaultCommand() was called */
+    private boolean initializedDefaultCommand = false;
+    /** The current command */
+    private Command currentCommand;
+    private boolean currentCommandChanged;
+
+    /** The default command */
+    private Command defaultCommand;
+    /** The name */
+    private String name;
+    /** List of all subsystems created */
+    private static Vector allSubsystems = new Vector();
+
+    /**
+     * Creates a subsystem with the given name
+     * @param name the name of the subsystem
+     */
+    public Subsystem(String name) {
+        this.name = name;
+        Scheduler.getInstance().registerSubsystem(this);
+    }
+
+    /**
+     * Creates a subsystem.  This will set the name to the name of the class.
+     */
+    public Subsystem() {
+        this.name = getClass().getName().substring(getClass().getName().lastIndexOf('.') + 1);
+        Scheduler.getInstance().registerSubsystem(this);
+        currentCommandChanged = true;
+    }
+
+    /**
+     * Initialize the default command for a subsystem
+     * By default subsystems have no default command, but if they do, the default command is set
+     * with this method. It is called on all Subsystems by CommandBase in the users program after
+     * all the Subsystems are created.
+     */
+    protected abstract void initDefaultCommand();
+
+    /**
+     * Sets the default command.  If this is not called or is called with null,
+     * then there will be no default command for the subsystem.
+     *
+     * <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
+     * singleton.</p>
+     *
+     * @param command the default command (or null if there should be none)
+     * @throws IllegalUseOfCommandException if the command does not require the subsystem
+     */
+    protected void setDefaultCommand(Command command) {
+        if (command == null) {
+            defaultCommand = null;
+        } else {
+            boolean found = false;
+            Enumeration requirements = command.getRequirements();
+            while (requirements.hasMoreElements()) {
+                if (requirements.nextElement().equals(this)) {
+                    found = true;
+//            } else {
+//                throw new IllegalUseOfCommandException("A default command cannot require multiple subsystems");
+                }
+            }
+            if (!found) {
+                throw new IllegalUseOfCommandException("A default command must require the subsystem");
+            }
+            defaultCommand = command;
+        }
+        if (table != null) {
+            if (defaultCommand != null) {
+                table.putBoolean("hasDefault", true);
+                table.putString("default", defaultCommand.getName());
+            } else {
+                table.putBoolean("hasDefault", false);
+            }
+        }
+    }
+
+    /**
+     * Returns the default command (or null if there is none).
+     * @return the default command
+     */
+    protected Command getDefaultCommand() {
+        if (!initializedDefaultCommand) {
+            initializedDefaultCommand = true;
+            initDefaultCommand();
+        }
+        return defaultCommand;
+    }
+
+    /**
+     * Sets the current command
+     * @param command the new current command
+     */
+    void setCurrentCommand(Command command) {
+        currentCommand = command;
+        currentCommandChanged = true;
+    }
+
+    /**
+     * Call this to alert Subsystem that the current command is actually the command.
+     * Sometimes, the {@link Subsystem} is told that it has no command while the {@link Scheduler}
+     * is going through the loop, only to be soon after given a new one.  This will avoid that situation.
+     */
+    void confirmCommand() {
+        if (currentCommandChanged) {
+            if (table != null) {
+                if (currentCommand != null) {
+                    table.putBoolean("hasCommand", true);
+                    table.putString("command", currentCommand.getName());
+                } else {
+                    table.putBoolean("hasCommand", false);
+                }
+            }
+            currentCommandChanged = false;
+        }
+    }
+
+    /**
+     * Returns the command which currently claims this subsystem.
+     * @return the command which currently claims this subsystem
+     */
+    public Command getCurrentCommand() {
+        return currentCommand;
+    }
+
+    public String toString() {
+        return getName();
+    }
+
+    /**
+     * Returns the name of this subsystem, which is by default the class name.
+     * @return the name of this subsystem
+     */
+    public String getName() {
+        return name;
+    }
+
+    public String getSmartDashboardType() {
+        return "Subsystem";
+    }
+    private ITable table;
+    public void initTable(ITable table) {
+        this.table = table;
+        if(table!=null) {
+            if (defaultCommand != null) {
+                table.putBoolean("hasDefault", true);
+                table.putString("default", defaultCommand.getName());
+            } else {
+                table.putBoolean("hasDefault", false);
+            }
+            if (currentCommand != null) {
+                table.putBoolean("hasCommand", true);
+                table.putString("command", currentCommand.getName());
+            } else {
+                table.putBoolean("hasCommand", false);
+            }
+        }
+    }
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return table;
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java

@@ -0,0 +1,50 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * A {@link WaitCommand} will wait for a certain amount of time before finishing.
+ * It is useful if you want a {@link CommandGroup} to pause for a moment.
+ * @author Joe Grinstead
+ * @see CommandGroup
+ */
+public class WaitCommand extends Command {
+
+    /**
+     * Instantiates a {@link WaitCommand} with the given timeout.
+     * @param timeout the time the command takes to run
+     */
+    public WaitCommand(double timeout) {
+        this("Wait(" + timeout + ")", timeout);
+    }
+
+    /**
+     * Instantiates a {@link WaitCommand} with the given timeout.
+     * @param name the name of the command
+     * @param timeout the time the command takes to run
+     */
+    public WaitCommand(String name, double timeout) {
+        super(name, timeout);
+    }
+
+    protected void initialize() {
+    }
+
+    protected void execute() {
+    }
+
+    protected boolean isFinished() {
+        return isTimedOut();
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.command;
+
+/**
+ * This command will only finish if whatever {@link CommandGroup} it is in has no active children.
+ * If it is not a part of a {@link CommandGroup}, then it will finish immediately.  If it is itself an
+ * active child, then the {@link CommandGroup} will never end.
+ *
+ * <p>This class is useful for the situation where you want to allow anything running in parallel to finish, before continuing
+ * in the main {@link CommandGroup} sequence.</p>
+ * @author Joe Grinstead
+ */
+public class WaitForChildren extends Command {
+
+    protected void initialize() {
+    }
+
+    protected void execute() {
+    }
+
+    protected void end() {
+    }
+
+    protected void interrupted() {
+    }
+
+    protected boolean isFinished() {
+        return getGroup() == null || getGroup().m_children.isEmpty();
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java

@@ -0,0 +1,7 @@
+package edu.wpi.first.wpilibj.interfaces;
+
+import edu.wpi.first.wpilibj.PIDSource;
+
+public interface Potentiometer extends PIDSource {
+	double get();
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java

@@ -0,0 +1,205 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.wpilibj.command.Scheduler;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.tables.ITable;
+import java.util.Enumeration;
+import java.util.Hashtable;
+import java.util.Vector;
+
+/**
+ * A LiveWindow component is a device (sensor or actuator) that should be added to the
+ * SmartDashboard in test mode. The components are cached until the first time the robot
+ * enters Test mode. This allows the components to be inserted, then renamed.
+ * @author brad
+ */
+class LiveWindowComponent {
+
+    String m_subsystem;
+    String m_name;
+    boolean m_isSensor;
+
+    public LiveWindowComponent(String subsystem, String name, boolean isSensor) {
+        m_subsystem = subsystem;
+        m_name = name;
+        m_isSensor = isSensor;
+    }
+
+    public String getName() {
+        return m_name;
+    }
+
+    public String getSubsystem() {
+        return m_subsystem;
+    }
+
+    public boolean isSensor() {
+        return m_isSensor;
+    }
+}
+
+/**
+ * The LiveWindow class is the public interface for putting sensors and
+ * actuators on the LiveWindow.
+ *
+ * @author Alex Henning
+ */
+public class LiveWindow {
+
+    private static Vector sensors = new Vector();
+//    private static Vector actuators = new Vector();
+    private static Hashtable components = new Hashtable();
+    private static ITable livewindowTable = NetworkTable.getTable("LiveWindow");
+    private static ITable statusTable = livewindowTable.getSubTable("~STATUS~");
+    private static boolean liveWindowEnabled = false;
+    private static boolean firstTime = true;
+
+    /**
+     * Initialize all the LiveWindow elements the first time we enter LiveWindow
+     * mode. By holding off creating the NetworkTable entries, it allows them to
+     * be redefined before the first time in LiveWindow mode. This allows
+     * default sensor and actuator values to be created that are replaced with
+     * the custom names from users calling addActuator and addSensor.
+     */
+    private static void initializeLiveWindowComponents() {
+        System.out.println("Initializing the components first time");
+        for (Enumeration e = components.keys(); e.hasMoreElements();) {
+            LiveWindowSendable component = (LiveWindowSendable) e.nextElement();
+            LiveWindowComponent c = (LiveWindowComponent) components.get(component);
+            String subsystem = c.getSubsystem();
+            String name = c.getName();
+            System.out.println("Initializing table for '" + subsystem + "' '" + name + "'");
+            livewindowTable.getSubTable(subsystem).putString("~TYPE~", "LW Subsystem");
+            ITable table = livewindowTable.getSubTable(subsystem).getSubTable(name);
+            table.putString("~TYPE~", component.getSmartDashboardType());
+            table.putString("Name", name);
+            table.putString("Subsystem", subsystem);
+            component.initTable(table);
+            if (c.isSensor()) {
+                sensors.addElement(component);
+            }
+        }
+    }
+
+    /**
+     * Set the enabled state of LiveWindow. If it's being enabled, turn off the
+     * scheduler and remove all the commands from the queue and enable all the
+     * components registered for LiveWindow. If it's being disabled, stop all
+     * the registered components and reenable the scheduler. TODO: add code to
+     * disable PID loops when enabling LiveWindow. The commands should reenable
+     * the PID loops themselves when they get rescheduled. This prevents arms
+     * from starting to move around, etc. after a period of adjusting them in
+     * LiveWindow mode.
+     */
+    public static void setEnabled(boolean enabled) {
+        if (liveWindowEnabled != enabled) {
+            if (enabled) {
+                System.out.println("Starting live window mode.");
+                if (firstTime) {
+                    initializeLiveWindowComponents();
+                    firstTime = false;
+                }
+                Scheduler.getInstance().disable();
+                Scheduler.getInstance().removeAll();
+                for (Enumeration e = components.keys(); e.hasMoreElements();) {
+                    LiveWindowSendable component = (LiveWindowSendable) e.nextElement();
+                    component.startLiveWindowMode();
+                }
+            } else {
+                System.out.println("stopping live window mode.");
+                for (Enumeration e = components.keys(); e.hasMoreElements();) {
+                    LiveWindowSendable component = (LiveWindowSendable) e.nextElement();
+                    component.stopLiveWindowMode();
+                }
+                Scheduler.getInstance().enable();
+            }
+            liveWindowEnabled = enabled;
+            statusTable.putBoolean("LW Enabled", enabled);
+        }
+    }
+
+    /**
+     * The run method is called repeatedly to keep the values refreshed on the screen in
+     * test mode.
+     */
+    public static void run() {
+        updateValues();
+    }
+
+    /**
+     * Add a Sensor associated with the subsystem and with call it by the given
+     * name.
+     *
+     * @param subsystem The subsystem this component is part of.
+     * @param name The name of this component.
+     * @param component A LiveWindowSendable component that represents a sensor.
+     */
+    public static void addSensor(String subsystem, String name, LiveWindowSendable component) {
+        components.put(component, new LiveWindowComponent(subsystem, name, true));
+    }
+
+    /**
+     * Add an Actuator associated with the subsystem and with call it by the
+     * given name.
+     *
+     * @param subsystem The subsystem this component is part of.
+     * @param name The name of this component.
+     * @param component A LiveWindowSendable component that represents a
+     * actuator.
+     */
+    public static void addActuator(String subsystem, String name, LiveWindowSendable component) {
+        components.put(component, new LiveWindowComponent(subsystem, name, false));
+    }
+
+    /**
+     * Puts all sensor values on the live window.
+     */
+    private static void updateValues() {
+        //TODO: gross - needs to be sped up
+        for (int i = 0; i < sensors.size(); i++) {
+            LiveWindowSendable lws = (LiveWindowSendable) sensors.elementAt(i);
+            lws.updateTable();
+        }
+        // TODO: Add actuators?
+        // TODO: Add better rate limiting.
+    }
+
+    /**
+     * Add Sensor to LiveWindow. The components are shown with the module type,
+     * slot and channel like this: Gyro[1, 2] for a gyro object connected to the
+     * first analog module in channel 2
+     *
+     * @param moduleType A string indicating the type of the module used in the
+     * naming (above)
+     * @param moduleNumber The number of the particular module type
+     * @param channel The channel number the device is connected to
+     * @param component A reference to the object being added
+     */
+    public static void addSensor(String moduleType, int moduleNumber, int channel, LiveWindowSendable component) {
+        addSensor("Ungrouped", moduleType + "[" + moduleNumber + "," + channel + "]", component);
+        if (sensors.contains(component)) {
+            sensors.removeElement(component);
+        }
+        sensors.addElement(component);
+    }
+
+    /**
+     * Add Actuator to LiveWindow. The components are shown with the module
+     * type, slot and channel like this: Servo[1,2] for a servo object connected
+     * to the first digital module and PWM port 2.
+     *
+     * @param moduleType A string that defines the module name in the label for
+     * the value
+     * @param moduleNumber The number of the particular module type
+     * @param channel The channel number the device is plugged into (usually
+     * PWM)
+     * @param component The reference to the object being added
+     */
+    public static void addActuator(String moduleType, int moduleNumber, int channel, LiveWindowSendable component) {
+        addActuator("Ungrouped", moduleType + "[" + moduleNumber + "," + channel + "]", component);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java

@@ -0,0 +1,32 @@
+/*
+ * To change this template, choose Tools | Templates
+ * and open the template in the editor.
+ */
+package edu.wpi.first.wpilibj.livewindow;
+
+import edu.wpi.first.wpilibj.Sendable;
+
+/**
+ * Live Window Sendable is a special type of object sendable to the live window.
+ *
+ * @author Alex Henning
+ */
+public interface LiveWindowSendable extends Sendable {
+    /**
+     * Update the table for this sendable object with the latest
+     * values.
+     */
+    public void updateTable();
+
+    /**
+     * Start having this sendable object automatically respond to
+     * value changes reflect the value on the table.
+     */
+    public void startLiveWindowMode();
+
+    /**
+     * Stop having this sendable object automatically respond to value
+     * changes.
+     */
+    public void stopLiveWindowMode();
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/MainNode.java

@@ -0,0 +1,33 @@
+package edu.wpi.first.wpilibj.simulation;
+
+import org.gazebosim.transport.Node;
+import org.gazebosim.transport.Publisher;
+import org.gazebosim.transport.Subscriber;
+import org.gazebosim.transport.SubscriberCallback;
+
+import com.google.protobuf.Message;
+
+public class MainNode{
+	private Node main;
+	
+	private MainNode() {
+		main = new Node("frc");
+	}
+	
+	private static MainNode instance;
+	public static MainNode getInstance() {
+		if (instance == null) {
+			instance = new MainNode();
+		}
+		return instance;
+	}
+	
+	public static <T extends Message> Publisher<T> advertise(String topic, T defaultMessage) {
+		return getInstance().main.advertise(topic, defaultMessage);
+	}
+
+	public static <T extends Message> Subscriber<T>
+			subscribe(String topic, T defaultMessage, SubscriberCallback<T> cb) {
+		return getInstance().main.subscribe(topic, defaultMessage, cb);
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimDigitalInput.java

@@ -0,0 +1,24 @@
+package edu.wpi.first.wpilibj.simulation;
+
+import gazebo.msgs.GzBool.Bool;
+
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.SubscriberCallback;
+
+public class SimDigitalInput {
+	private boolean value;
+	
+	public SimDigitalInput(String topic) {
+    	MainNode.subscribe(topic, Msgs.Bool(),
+			new SubscriberCallback<Bool>() {
+    			@Override public void callback(Bool msg) {
+    				value = msg.getData();
+    			}
+			}
+		);
+	}
+
+	public boolean get() {
+		return value;
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimEncoder.java

@@ -0,0 +1,58 @@
+package edu.wpi.first.wpilibj.simulation;
+
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.Publisher;
+import org.gazebosim.transport.SubscriberCallback;
+
+import gazebo.msgs.GzFloat64.Float64;
+import gazebo.msgs.GzString;
+
+public class SimEncoder {
+	private double position, velocity;
+	private Publisher<GzString.String> command_pub;
+	
+	public SimEncoder(String topic) {
+		command_pub = MainNode.advertise(topic+"/control", Msgs.String());
+		command_pub.setLatchMode(true);
+
+		MainNode.subscribe(topic+"/position", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override public void callback(Float64 msg) {
+					position = msg.getData();
+				}
+			}
+    	);
+    	
+    	MainNode.subscribe(topic+"/velocity", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override public void callback(Float64 msg) {
+					velocity = msg.getData();
+				}
+			}
+		);
+	}
+	
+	public void reset() {
+		sendCommand("reset");
+	}
+
+	public void start() {
+		sendCommand("start");
+	}
+
+	public void stop() {
+		sendCommand("stop");
+	}
+
+	private void sendCommand(String cmd) {
+		command_pub.publish(Msgs.String(cmd));
+	}
+
+	public double getPosition() {
+		return position;
+	}
+
+	public double getVelocity() {
+		return velocity;
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimFloatInput.java

@@ -0,0 +1,25 @@
+package edu.wpi.first.wpilibj.simulation;
+
+
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.SubscriberCallback;
+
+import gazebo.msgs.GzFloat64.Float64;
+
+public class SimFloatInput {
+	private double value;
+	
+	public SimFloatInput(String topic) {
+    	MainNode.subscribe(topic, Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+    			@Override public void callback(Float64 msg) {
+    				value = msg.getData();
+    			}
+			}
+		);
+	}
+
+	public double get() {
+		return value;
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimGyro.java

@@ -0,0 +1,50 @@
+package edu.wpi.first.wpilibj.simulation;
+
+import gazebo.msgs.GzFloat64.Float64;
+import gazebo.msgs.GzString;
+
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.Publisher;
+import org.gazebosim.transport.SubscriberCallback;
+
+public class SimGyro {
+	private double position, velocity;
+	private Publisher<GzString.String> command_pub;
+	
+	public SimGyro(String topic) {
+		command_pub = MainNode.advertise(topic+"/control", Msgs.String());
+		command_pub.setLatchMode(true);
+
+		MainNode.subscribe(topic+"/position", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override public void callback(Float64 msg) {
+					position = msg.getData();
+				}
+			}
+    	);
+    	
+    	MainNode.subscribe(topic+"/velocity", Msgs.Float64(),
+			new SubscriberCallback<Float64>() {
+				@Override public void callback(Float64 msg) {
+					velocity = msg.getData();
+				}
+			}
+		);
+	}
+	
+	public void reset() {
+		sendCommand("reset");
+	}
+
+	private void sendCommand(String cmd) {
+		command_pub.publish(Msgs.String(cmd));
+	}
+
+	public double getAngle() {
+		return position;
+	}
+
+	public double getVelocity() {
+		return velocity;
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/simulation/SimSpeedController.java

@@ -0,0 +1,73 @@
+package edu.wpi.first.wpilibj.simulation;
+
+import gazebo.msgs.GzFloat64.Float64;
+
+import org.gazebosim.transport.Msgs;
+import org.gazebosim.transport.Publisher;
+
+
+public class SimSpeedController {
+	private Publisher<Float64> pub;
+	private double speed;
+
+    /**
+     * Constructor that assumes the default digital module.
+     *
+     * @param channel The PWM channel on the digital module that the Victor is attached to.
+     */
+    public SimSpeedController(String topic) {
+    	pub = MainNode.advertise(topic, Msgs.Float64());
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * @deprecated For compatibility with CANJaguar
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed to set.  Value should be between -1.0 and 1.0.
+     * @param syncGroup The update group to add this Set() to, pending UpdateSyncGroup().  If 0, update immediately.
+     */
+    public void set(double speed, byte syncGroup) {
+        set(speed);
+    }
+
+    /**
+     * Set the PWM value.
+     *
+     * The PWM value is set using a range of -1.0 to 1.0, appropriately
+     * scaling the value for the FPGA.
+     *
+     * @param speed The speed value between -1.0 and 1.0 to set.
+     */
+    public void set(double speed) {
+		pub.publish(Msgs.Float64(speed));
+    }
+
+    /**
+     * Get the recently set value of the PWM.
+     *
+     * @return The most recently set value for the PWM between -1.0 and 1.0.
+     */
+    public double get() {
+        return speed;
+    }
+	
+    /**
+     * Disable the speed controller
+     */
+    public void disable() {
+		set(0);
+	}
+
+    /**
+     * Write out the PID value as seen in the PIDOutput base object.
+     *
+     * @param output Write out the PWM value as was found in the PIDController
+     */
+    public void pidWrite(double output) {
+        set(output);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java

@@ -0,0 +1,146 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.command.Command;
+import edu.wpi.first.wpilibj.networktables2.type.StringArray;
+import edu.wpi.first.wpilibj.networktables2.util.List;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * The {@link SendableChooser} class is a useful tool for presenting a selection
+ * of options to the {@link SmartDashboard}.
+ *
+ * <p>For instance, you may wish to be able to select between multiple
+ * autonomous modes. You can do this by putting every possible {@link Command}
+ * you want to run as an autonomous into a {@link SendableChooser} and then put
+ * it into the {@link SmartDashboard} to have a list of options appear on the
+ * laptop. Once autonomous starts, simply ask the {@link SendableChooser} what
+ * the selected value is.</p>
+ *
+ * @author Joe Grinstead
+ */
+public class SendableChooser implements Sendable {
+
+    /**
+     * The key for the default value
+     */
+    private static final String DEFAULT = "default";
+    /**
+     * The key for the selected option
+     */
+    private static final String SELECTED = "selected";
+    /**
+     * The key for the option array
+     */
+    private static final String OPTIONS = "options";
+    /**
+     * A table linking strings to the objects the represent
+     */
+    private StringArray choices = new StringArray();
+    private List values = new List();
+    private String defaultChoice = null;
+    private Object defaultValue = null;
+
+    /**
+     * Instantiates a {@link SendableChooser}.
+     */
+    public SendableChooser() {
+    }
+
+    /**
+     * Adds the given object to the list of options. On the
+     * {@link SmartDashboard} on the desktop, the object will appear as the
+     * given name.
+     *
+     * @param name the name of the option
+     * @param object the option
+     */
+    public void addObject(String name, Object object) {
+        //if we don't have a default, set the default automatically
+        if (defaultChoice == null) {
+            addDefault(name, object);
+            return;
+        }
+        for (int i = 0; i < choices.size(); ++i) {
+            if (choices.get(i).equals(name)) {
+                choices.set(i, name);
+                values.set(i, object);
+                return;
+            }
+        }
+        //not found
+        choices.add(name);
+        values.add(object);
+        if (table != null) {
+            table.putValue(OPTIONS, choices);
+        }
+    }
+
+    /**
+     * Add the given object to the list of options and marks it as the default.
+     * Functionally, this is very close to
+     * {@link SendableChooser#addObject(java.lang.String, java.lang.Object) addObject(...)}
+     * except that it will use this as the default option if none other is
+     * explicitly selected.
+     *
+     * @param name the name of the option
+     * @param object the option
+     */
+    public void addDefault(String name, Object object) {
+        if (name == null) {
+            throw new NullPointerException("Name cannot be null");
+        }
+        defaultChoice = name;
+        defaultValue = object;
+        if (table != null) {
+            table.putString(DEFAULT, defaultChoice);
+        }
+        addObject(name, object);
+    }
+
+    /**
+     * Returns the selected option. If there is none selected, it will return
+     * the default. If there is none selected and no default, then it will
+     * return {@code null}.
+     *
+     * @return the option selected
+     */
+    public Object getSelected() {
+        String selected = table.getString(SELECTED, null);
+        for (int i = 0; i < values.size(); ++i) {
+            if (choices.get(i).equals(selected)) {
+                return values.get(i);
+            }
+        }
+        return defaultValue;
+
+    }
+
+    public String getSmartDashboardType() {
+        return "String Chooser";
+    }
+    private ITable table;
+
+    public void initTable(ITable table) {
+        this.table = table;
+        if (table != null) {
+            table.putValue(OPTIONS, choices);
+            if (defaultChoice != null) {
+                table.putString(DEFAULT, defaultChoice);
+            }
+        }
+    }
+
+    /**
+     * {@inheritDoc}
+     */
+    public ITable getTable() {
+        return table;
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java

@@ -0,0 +1,297 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.wpilibj.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.networktables.NetworkTableKeyNotDefined;
+import edu.wpi.first.wpilibj.tables.ITable;
+import edu.wpi.first.wpilibj.tables.TableKeyNotDefinedException;
+import java.util.Hashtable;
+import java.util.NoSuchElementException;
+
+/**
+ * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on the
+ * laptop.
+ *
+ * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the laptop.
+ * Users can put values into and get values from the SmartDashboard</p>
+ *
+ * @author Joe Grinstead
+ */
+public class SmartDashboard {
+    //TODO usage reporting
+    /** The {@link NetworkTable} used by {@link SmartDashboard} */
+    private static final NetworkTable table = NetworkTable.getTable("SmartDashboard");
+    /**
+     * A table linking tables in the SmartDashboard to the {@link SmartDashboardData} objects
+     * they came from.
+     */
+    private static final Hashtable tablesToData = new Hashtable();
+
+    /**
+     * Maps the specified key to the specified value in this table.
+     * The key can not be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     * @param key the key
+     * @param data the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putData(String key, Sendable data) {
+        ITable dataTable = table.getSubTable(key);
+        dataTable.putString("~TYPE~", data.getSmartDashboardType());
+        data.initTable(dataTable);
+        tablesToData.put(data, key);
+    }
+
+
+    //TODO should we reimplement NamedSendable?
+    /**
+     * Maps the specified key (where the key is the name of the {@link SmartDashboardNamedData}
+     * to the specified value in this table.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     * @param value the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putData(NamedSendable value) {
+        putData(value.getName(), value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @return the value
+     * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a {@link SmartDashboardData}
+     * @throws IllegalArgumentException if the key is null
+     */
+//TODO    public static SmartDashboardData getData(String key) {
+//        NetworkTable subtable = table.getSubTable(key);
+//        Object data = tablesToData.get(subtable);
+//        if (data == null) {
+//            throw new IllegalArgumentException("Value at \"" + key + "\" is not a boolean");
+//        } else {
+//            return (SmartDashboardData) data;
+//        }
+//    }
+
+    /**
+     * Maps the specified key to the specified value in this table.
+     * The key can not be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     * @param key the key
+     * @param value the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putBoolean(String key, boolean value) {
+        table.putBoolean(key, value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @return the value
+     * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static boolean getBoolean(String key) throws TableKeyNotDefinedException {
+        return table.getBoolean(key);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @param defaultValue returned if the key doesn't exist
+     * @return the value
+     * @throws IllegalArgumentException if the value mapped to by the key is not a boolean
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static boolean getBoolean(String key, boolean defaultValue) {
+        return table.getBoolean(key, defaultValue);
+    }
+
+    /**
+     * Maps the specified key to the specified value in this table.
+     * The key can not be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     * @param key the key
+     * @param value the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putNumber(String key, double value) {
+        table.putNumber(key, value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @return the value
+     * @throws TableKeyNotDefinedException if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a double
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static double getNumber(String key) throws TableKeyNotDefinedException {
+        return table.getNumber(key);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @param defaultValue the value returned if the key is undefined
+     * @return the value
+     * @throws NoSuchEleNetworkTableKeyNotDefinedmentException if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a double
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static double getNumber(String key, double defaultValue) {
+        return table.getNumber(key, defaultValue);
+    }
+
+    /**
+     * Maps the specified key to the specified value in this table.
+     * Neither the key nor the value can be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     * @param key the key
+     * @param value the value
+     * @throws IllegalArgumentException if key or value is null
+     */
+    public static void putString(String key, String value) {
+        table.putString(key, value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     * @param key the key
+     * @return the value
+     * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a string
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static String getString(String key) throws TableKeyNotDefinedException {
+        return table.getString(key);
+    }
+
+    /**
+    * Returns the value at the specified key.
+    * @param key the key
+    * @param defaultValue  The value returned if the key is undefined
+    * @return the value
+    * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
+    * @throws IllegalArgumentException if the value mapped to by the key is not a string
+    * @throws IllegalArgumentException if the key is null
+    */
+    public static String getString(String key, String defaultValue) {
+        return table.getString(key, defaultValue);
+    }
+
+
+
+
+
+
+
+
+
+    /*
+     * Deprecated Methods
+     */
+    /**
+     * Maps the specified key to the specified value in this table.
+     *
+     * The key can not be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     *
+     * @deprecated Use {@link #putNumber(java.lang.String, double) putNumber method} instead
+     * @param key the key
+     * @param value the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putInt(String key, int value) {
+        table.putNumber(key, value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     *
+     * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
+     * @param key the key
+     * @return the value
+     * @throws TableKeyNotDefinedException if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not an int
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static int getInt(String key) throws TableKeyNotDefinedException {
+        return (int) table.getNumber(key);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     *
+     * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead
+     * @param key the key
+     * @param defaultValue the value returned if the key is undefined
+     * @return the value
+     * @throws NetworkTableKeyNotDefined if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not an int
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static int getInt(String key, int defaultValue) throws TableKeyNotDefinedException {
+        try {
+            return (int) table.getNumber(key);
+        } catch (NoSuchElementException ex) {
+            return defaultValue;
+        }
+    }
+
+    /**
+     * Maps the specified key to the specified value in this table.
+     *
+     * The key can not be null.
+     * The value can be retrieved by calling the get method with a key that is equal to the original key.
+     *
+     * @deprecated Use{@link #putNumber(java.lang.String, double) putNumber} instead
+     * @param key the key
+     * @param value the value
+     * @throws IllegalArgumentException if key is null
+     */
+    public static void putDouble(String key, double value) {
+        table.putNumber(key, value);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     *
+     * @deprecated Use {@link #getNumber(java.lang.String) getNumber} instead
+     * @param key the key
+     * @return the value
+     * @throws NoSuchEleNetworkTableKeyNotDefinedmentException if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a double
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static double getDouble(String key) throws TableKeyNotDefinedException {
+        return table.getNumber(key);
+    }
+
+    /**
+     * Returns the value at the specified key.
+     *
+     * @deprecated Use {@link #getNumber(java.lang.String, double) getNumber} instead.
+     * @param key the key
+     * @param defaultValue the value returned if the key is undefined
+     * @return the value
+     * @throws NoSuchEleNetworkTableKeyNotDefinedmentException if there is no value mapped to by the key
+     * @throws IllegalArgumentException if the value mapped to by the key is not a double
+     * @throws IllegalArgumentException if the key is null
+     */
+    public static double getDouble(String key, double defaultValue) {
+        return table.getNumber(key, defaultValue);
+    }
+
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/util/AllocationException.java

@@ -0,0 +1,23 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Exception indicating that the resource is already allocated
+ * @author dtjones
+ */
+public class AllocationException extends RuntimeException {
+
+    /**
+     * Create a new AllocationException
+     * @param msg the message to attach to the exception
+     */
+    public AllocationException(String msg) {
+        super(msg);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/util/BoundaryException.java

@@ -0,0 +1,62 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * This exception represents an error in which a lower limit was set as higher
+ * than an upper limit.
+ * 
+ * @author dtjones
+ */
+public class BoundaryException extends RuntimeException {
+
+	/**
+	 * Create a new exception with the given message
+	 * 
+	 * @param message
+	 *            the message to attach to the exception
+	 */
+	public BoundaryException(String message) {
+		super(message);
+	}
+
+	/**
+	 * Make sure that the given value is between the upper and lower bounds, and
+	 * throw an exception if they are not.
+	 * 
+	 * @param value
+	 *            The value to check.
+	 * @param lower
+	 *            The minimum acceptable value.
+	 * @param upper
+	 *            The maximum acceptable value.
+	 */
+	public static void assertWithinBounds(double value, double lower,
+			double upper) {
+		if (value < lower || value > upper)
+			throw new BoundaryException("Value must be between " + lower
+					+ " and " + upper + ", " + value + " given");
+	}
+
+	/**
+	 * Returns the message for a boundary exception. Used to keep the message
+	 * consistent across all boundary exceptions.
+	 * 
+	 * @param value
+	 *            The given value
+	 * @param lower
+	 *            The lower limit
+	 * @param upper
+	 *            The upper limit
+	 * @return
+	 */
+	public static String getMessage(double value, double lower, double upper) {
+		return "Value must be between " + lower + " and " + upper + ", "
+				+ value + " given";
+	}
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/util/CheckedAllocationException.java

@@ -0,0 +1,24 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Exception indicating that the resource is already allocated
+ * This is meant to be thrown by the resource class
+ * @author dtjones
+ */
+public class CheckedAllocationException extends Exception {
+
+    /**
+     * Create a new CheckedAllocationException
+     * @param msg the message to attach to the exception
+     */
+    public CheckedAllocationException(String msg) {
+        super(msg);
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/util/SortedVector.java

@@ -0,0 +1,75 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.util;
+
+import java.util.Vector;
+
+/**
+ *
+ * @author dtjones
+ */
+public class SortedVector extends Vector {
+
+    /**
+     * Interface used to determine the order to place sorted objects.
+     */
+    public static interface Comparator {
+
+        /**
+         * Compare the given two objects.
+         * @param object1 First object to compare
+         * @param object2 Second object to compare
+         * @return -1, 0, or 1 if the first object is less than, equal to, or
+         *  greater than the second, respectively
+         */
+        public int compare(Object object1, Object object2);
+    }
+    Comparator comparator;
+
+    /**
+     * Create a new sorted vector and use the given comparator to determine order.
+     * @param comparator The comparator to use to determine what order to place
+     * the elements in this vector.
+     */
+    public SortedVector(Comparator comparator) {
+        this.comparator = comparator;
+    }
+
+    /**
+     * Adds an element in the Vector, sorted from greatest to least.
+     * @param element The element to add to the Vector
+     */
+    public void addElement(Object element) {
+        int highBound = size();
+        int lowBound = 0;
+        while (highBound - lowBound > 0) {
+            int index = (highBound + lowBound) / 2;
+            int result = comparator.compare(element, elementAt(index));
+            if (result < 0) {
+                lowBound = index + 1;
+            } else if (result > 0) {
+                highBound = index;
+            } else {
+                lowBound = index;
+                highBound = index;
+            }
+        }
+        insertElementAt(element, lowBound);
+    }
+
+    /**
+     * Sort the vector.
+     */
+    public void sort() {
+        Object[] array = new Object[size()];
+        copyInto(array);
+        removeAllElements();
+        for (int i = 0; i < array.length; i++) {
+            addElement(array[i]);
+        }
+    }
+}

wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/util/UncleanStatusException.java

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2012. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Exception for bad status codes from the chip object
+ * @author Brian
+ */
+public final class UncleanStatusException extends IllegalStateException {
+
+    private final int statusCode;
+
+    /**
+     * Create a new UncleanStatusException
+     * @param status the status code that caused the exception
+     * @param message A message describing the exception
+     */
+    public UncleanStatusException(int status, String message) {
+        super(message);
+        statusCode = status;
+    }
+
+    /**
+     * Create a new UncleanStatusException
+     * @param status the status code that caused the exception
+     */
+    public UncleanStatusException(int status) {
+        this(status, "Status code was non-zero");
+    }
+
+    /**
+     * Create a new UncleanStatusException
+     * @param message a message describing the exception
+     */
+    public UncleanStatusException(String message) {
+        this(-1, message);
+    }
+
+    /**
+     * Create a new UncleanStatusException
+     */
+    public UncleanStatusException() {
+        this(-1, "Status code was non-zero");
+    }
+
+    /**
+     * Create a new UncleanStatusException
+     * @return the status code that caused the exception
+     */
+    public int getStatus() {
+        return statusCode;
+    }
+}