WPILib Reorganization

This is a major restructuring of the WPILib repository to simply build
procedures and remove the remnants of Maven from everything except the
eclipse plugins. Gradle files have been largely simplified or rewritten,
taking advantage of splitting up parts of the build into separate build
files for ease of reading.

The eclipse plugins are now in a separate project, as is ntcore. All
dependencies are resolved via Maven dependencies, with the
Jenkins-maintained WPILib repo. Project structures have also been
simplified: we no longer have separate subprojects inside wpilibc and
wpilibj. Where possible, these changes hav been done with git renames,
to make sure we still have full history for all repositories. Other
unrelated subprojects have also been broken out: OutlineViewer is now a
separate project.

Change-Id: Ib4e2a6e1a2f66427a14f16612b0e0d69ed661878

wpilibj/src/shared/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/src/shared/java/edu/wpi/first/wpilibj/GenericHID.java

@@ -0,0 +1,177 @@
+/*----------------------------------------------------------------------------*/
+/* 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);
+
+  public abstract int getPOV(int pov);
+
+  public int getPOV() {
+    return getPOV(0);
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/HLUsageReporting.java

@@ -0,0 +1,56 @@
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.BaseSystemNotInitializedException;
+
+/**
+ * Support for high level usage reporting.
+ *
+ * @author alex
+ */
+public class HLUsageReporting {
+  private static Interface impl;
+
+  public static void SetImplementation(Interface i) {
+    impl = i;
+  }
+
+  public static void reportScheduler() {
+    if (impl != null) {
+      impl.reportScheduler();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, HLUsageReporting.class);
+    }
+  }
+
+  public static void reportPIDController(int num) {
+    if (impl != null) {
+      impl.reportPIDController(num);
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, HLUsageReporting.class);
+    }
+  }
+
+  public static void reportSmartDashboard() {
+    if (impl != null) {
+      impl.reportSmartDashboard();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, HLUsageReporting.class);
+    }
+  }
+
+  public interface Interface {
+    void reportScheduler();
+
+    void reportPIDController(int num);
+
+    void reportSmartDashboard();
+  }
+
+  public static class Null implements Interface {
+    public void reportScheduler() {}
+
+    public void reportPIDController(int num) {}
+
+    public void reportSmartDashboard() {}
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/MotorSafety.java

@@ -0,0 +1,30 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/MotorSafetyHelper.java

@@ -0,0 +1,132 @@
+/*----------------------------------------------------------------------------*/
+/* 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.Timer;
+
+/**
+ * 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() {
+    if (!m_enabled || RobotState.isDisabled() || RobotState.isTest())
+      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/src/shared/java/edu/wpi/first/wpilibj/NamedSendable.java

@@ -0,0 +1,16 @@
+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/src/shared/java/edu/wpi/first/wpilibj/PIDController.java

@@ -0,0 +1,738 @@
+/* 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 java.util.LinkedList;
+
+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 PIDInterface, 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_prevInput = 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 int m_bufLength = 0;
+  private LinkedList<Double> m_buf;
+  private double m_bufTotal = 0.0;
+  private double m_setpoint = 0.0;
+  private double m_error = 0.0;
+  private double m_result = 0.0;
+  private double m_period = kDefaultPeriod;
+  protected PIDSource m_pidInput;
+  protected PIDOutput m_pidOutput;
+  java.util.Timer m_controlLoop;
+  private boolean m_freed = false;
+  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();
+  }
+
+  /**
+   * Used internally for when Tolerance hasn't been set.
+   */
+  public class NullTolerance implements Tolerance {
+    @Override
+    public boolean onTarget() {
+      throw new RuntimeException("No tolerance value set when calling onTarget().");
+    }
+  }
+
+  public class PercentageTolerance implements Tolerance {
+    double percentage;
+
+    PercentageTolerance(double value) {
+      percentage = value;
+    }
+
+    @Override
+    public boolean onTarget() {
+      return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
+    }
+  }
+
+  public class AbsoluteTolerance implements Tolerance {
+    double value;
+
+    AbsoluteTolerance(double value) {
+      this.value = value;
+    }
+
+    @Override
+    public boolean onTarget() {
+      return Math.abs(getAvgError()) < value;
+    }
+  }
+
+  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;
+    }
+
+    @Override
+    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++;
+    HLUsageReporting.reportPIDController(instances);
+    m_tolerance = new NullTolerance();
+
+    m_buf = new LinkedList<Double>();
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D and 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
+   * @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, 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 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
+   */
+  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();
+    synchronized (this) {
+      m_freed = true;
+      m_pidOutput = null;
+      m_pidInput = null;
+      m_controlLoop = null;
+    }
+    if (this.table != null)
+      table.removeTableListener(listener);
+  }
+
+  /**
+   * 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.
+   */
+  protected 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;
+      double result;
+      PIDOutput pidOutput = null;
+      synchronized (this) {
+        input = pidInput.pidGet();
+      }
+      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_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
+          if (m_P != 0) {
+            double potentialPGain = (m_totalError + m_error) * m_P;
+            if (potentialPGain < m_maximumOutput) {
+              if (potentialPGain > m_minimumOutput) {
+                m_totalError += m_error;
+              } else {
+                m_totalError = m_minimumOutput / m_P;
+              }
+            } else {
+              m_totalError = m_maximumOutput / m_P;
+            }
+
+            m_result = m_P * m_totalError + m_D * m_error + m_setpoint * m_F;
+          }
+        }
+        else {
+          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_prevInput - input) + m_setpoint * m_F;
+        }
+        m_prevInput = input;
+
+        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;
+
+        // Update the buffer.
+        m_buf.push(m_error);
+        m_bufTotal += m_error;
+        // Remove old elements when the buffer is full.
+        if (m_buf.size() > m_bufLength) {
+          m_bufTotal -= m_buf.pop();
+        }
+      }
+
+      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 synchronized double getP() {
+    return m_P;
+  }
+
+  /**
+   * Get the Integral coefficient
+   *$
+   * @return integral coefficient
+   */
+  public synchronized 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 and setpoint.
+   *
+   * @param minimumInput the minimum value expected from the input
+   * @param maximumInput the maximum value expected from the input
+   */
+  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
+   * Clears the queue for GetAvgError().
+   *$
+   * @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;
+    }
+
+    m_buf.clear();
+
+    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();
+  }
+
+  /**
+   * Sets what type of input the PID controller will use
+   *$
+   * @param pidSource the type of input
+   */
+  void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidInput.setPIDSourceType(pidSource);
+  }
+
+  /**
+   * Returns the type of input the PID controller is using
+   *$
+   * @return the PID controller input type
+   */
+  PIDSourceType getPIDSourceType() {
+    return m_pidInput.getPIDSourceType();
+  }
+
+  /**
+   * Returns the current difference of the error over the past few iterations.
+   * You can specify the number of iterations to average with
+   * setToleranceBuffer() (defaults to 1). getAvgError() is used for the
+   * onTarget() function.
+   *$
+   * @return the current average of the error
+   */
+  public synchronized double getAvgError() {
+    double avgError = 0;
+    // Don't divide by zero.
+    if (m_buf.size() != 0) avgError = m_bufTotal / m_buf.size();
+    return avgError;
+  }
+
+  /**
+   * 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 {@link #setPercentTolerance(double)} or
+   *             {@link #setAbsoluteTolerance(double)} instead.
+   */
+  @Deprecated
+  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
+   */
+  public void setTolerance(Tolerance tolerance) {
+    m_tolerance = tolerance;
+  }
+
+  /**
+   * Set the absolute error which is considered tolerable for use with OnTarget.
+   *$
+   * @param absvalue 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 percentage percent error which is tolerable
+   */
+  public synchronized void setPercentTolerance(double percentage) {
+    m_tolerance = new PercentageTolerance(percentage);
+  }
+
+  /**
+   * Set the number of previous error samples to average for tolerancing. When
+   * determining whether a mechanism is on target, the user may want to use a
+   * rolling average of previous measurements instead of a precise position or
+   * velocity. This is useful for noisy sensors which return a few erroneous
+   * measurements when the mechanism is on target. However, the mechanism will
+   * not register as on target for at least the specified bufLength cycles.
+   * @param bufLength Number of previous cycles to average.
+   */
+  public synchronized void setToleranceBuffer(int bufLength) {
+    m_bufLength = bufLength;
+
+    // Cut the existing buffer down to size if needed.
+    while (m_buf.size() > bufLength) {
+      m_bufTotal -= m_buf.pop();
+    }
+  }
+
+  /**
+   * 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
+   */
+  @Override
+  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.
+   */
+  @Override
+  public synchronized void disable() {
+    m_pidOutput.pidWrite(0);
+    m_enabled = false;
+
+    if (table != null) {
+      table.putBoolean("enabled", false);
+    }
+  }
+
+  /**
+   * Return true if PIDController is enabled.
+   *
+   * @deprecated Call {@link #isEnabled()} instead.
+   */
+  @Deprecated
+  public synchronized boolean isEnable() {
+    return isEnabled();
+  }
+
+  /**
+   * Return true if PIDController is enabled.
+   */
+  public boolean isEnabled() {
+    return m_enabled;
+  }
+
+  /**
+   * Reset the previous error,, the integral term, and disable the controller.
+   */
+  public synchronized void reset() {
+    disable();
+    m_prevInput = 0;
+    m_totalError = 0;
+    m_result = 0;
+  }
+
+  @Override
+  public String getSmartDashboardType() {
+    return "PIDController";
+  }
+
+  private final ITableListener listener = new ITableListener() {
+    @Override
+    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 (getP() != table.getNumber("p", 0.0) || getI() != table.getNumber("i", 0.0)
+            || getD() != table.getNumber("d", 0.0) || getF() != 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 (getSetpoint() != ((Double) value).doubleValue())
+          setSetpoint(((Double) value).doubleValue());
+      } else if (key.equals("enabled")) {
+        if (isEnable() != ((Boolean) value).booleanValue()) {
+          if (((Boolean) value).booleanValue()) {
+            enable();
+          } else {
+            disable();
+          }
+        }
+      }
+    }
+  };
+  private ITable table;
+
+  @Override
+  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}
+   */
+  @Override
+  public ITable getTable() {
+    return table;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void updateTable() {}
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void startLiveWindowMode() {
+    disable();
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void stopLiveWindowMode() {}
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDInterface.java

@@ -0,0 +1,26 @@
+package edu.wpi.first.wpilibj;
+
+public interface PIDInterface extends Controller {
+
+  public void setPID(double p, double i, double d);
+
+  public double getP();
+
+  public double getI();
+
+  public double getD();
+
+  public void setSetpoint(double setpoint);
+
+  public double getSetpoint();
+
+  public double getError();
+
+  public void enable();
+
+  public void disable();
+
+  public boolean isEnabled();
+
+  public void reset();
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDOutput.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;
+
+/**
+ * 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/src/shared/java/edu/wpi/first/wpilibj/PIDSource.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;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/**
+ * This interface allows for PIDController to automatically read from this
+ * object
+ *$
+ * @author dtjones
+ */
+public interface PIDSource {
+  /**
+   * Set which parameter of the device you are using as a process control
+   * variable.
+   *
+   * @param pidSource
+   *            An enum to select the parameter.
+   */
+  public void setPIDSourceType(PIDSourceType pidSource);
+
+  /**
+   * Get which parameter of the device you are using as a process control
+   * variable.
+   *
+   * @return the currently selected PID source parameter
+   */
+  public PIDSourceType getPIDSourceType();
+
+  /**
+   * Get the result to use in PIDController
+   *$
+   * @return the result to use in PIDController
+   */
+  public double pidGet();
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/PIDSourceType.java

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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;
+
+/**
+ * A description for the type of output value to provide to a PIDController
+ */
+public enum PIDSourceType {
+  kDisplacement(0),
+  kRate(1);
+
+  /**
+   * The integer value representing this enumeration
+   */
+  public final int value;
+
+  private PIDSourceType(int value) {
+    this.value = value;
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/RobotState.java

@@ -0,0 +1,63 @@
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.BaseSystemNotInitializedException;
+
+public class RobotState {
+  private static Interface impl;
+
+  public static void SetImplementation(Interface i) {
+    impl = i;
+  }
+
+  public static boolean isDisabled() {
+    if (impl != null) {
+      return impl.isDisabled();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, RobotState.class);
+    }
+  }
+
+  public static boolean isEnabled() {
+    if (impl != null) {
+      return impl.isEnabled();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, RobotState.class);
+    }
+  }
+
+  public static boolean isOperatorControl() {
+    if (impl != null) {
+      return impl.isOperatorControl();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, RobotState.class);
+    }
+  }
+
+  public static boolean isAutonomous() {
+    if (impl != null) {
+      return impl.isAutonomous();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, RobotState.class);
+    }
+  }
+
+  public static boolean isTest() {
+    if (impl != null) {
+      return impl.isTest();
+    } else {
+      throw new BaseSystemNotInitializedException(Interface.class, RobotState.class);
+    }
+  }
+
+  interface Interface {
+    boolean isDisabled();
+
+    boolean isEnabled();
+
+    boolean isOperatorControl();
+
+    boolean isAutonomous();
+
+    boolean isTest();
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/Sendable.java

@@ -0,0 +1,28 @@
+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/src/shared/java/edu/wpi/first/wpilibj/Timer.java

@@ -0,0 +1,172 @@
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.wpilibj.util.BaseSystemNotInitializedException;
+
+public class Timer {
+  private static StaticInterface impl;
+
+  public static void SetImplementation(StaticInterface ti) {
+    impl = ti;
+  }
+
+  /**
+   * 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() {
+    if (impl != null) {
+      return impl.getFPGATimestamp();
+    } else {
+      throw new BaseSystemNotInitializedException(StaticInterface.class, Timer.class);
+    }
+  }
+
+  /**
+   * 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 static double getMatchTime() {
+    if (impl != null) {
+      return impl.getMatchTime();
+    } else {
+      throw new BaseSystemNotInitializedException(StaticInterface.class, Timer.class);
+    }
+  }
+
+  /**
+   * 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) {
+    if (impl != null) {
+      impl.delay(seconds);
+    } else {
+      throw new BaseSystemNotInitializedException(StaticInterface.class, Timer.class);
+    }
+  }
+
+  public interface StaticInterface {
+    double getFPGATimestamp();
+
+    double getMatchTime();
+
+    void delay(final double seconds);
+
+    Interface newTimer();
+  }
+
+  private final Interface timer;
+
+  public Timer() {
+    if (impl != null) {
+      timer = impl.newTimer();
+    } else {
+      throw new BaseSystemNotInitializedException(StaticInterface.class, Timer.class);
+    }
+  }
+
+  /**
+   * 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 double get() {
+    return timer.get();
+  }
+
+  /**
+   * Reset the timer by setting the time to 0. Make the timer startTime the
+   * current time so new requests will be relative now
+   */
+  public void reset() {
+    timer.reset();
+  }
+
+  /**
+   * Start the timer running. Just set the running flag to true indicating that
+   * all time requests should be relative to the system clock.
+   */
+  public void start() {
+    timer.start();
+  }
+
+  /**
+   * 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 void stop() {
+    timer.stop();
+  }
+
+  /**
+   * Check if the period specified has passed and if it has, advance the start
+   * time by that period. This is useful to decide if it's time to do periodic
+   * work without drifting later by the time it took to get around to checking.
+   *
+   * @param period The period to check for (in seconds).
+   * @return If the period has passed.
+   */
+  public boolean hasPeriodPassed(double period) {
+    return timer.hasPeriodPassed(period);
+  }
+
+  public interface Interface {
+    /**
+     * 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 double get();
+
+    /**
+     * Reset the timer by setting the time to 0. Make the timer startTime the
+     * current time so new requests will be relative now
+     */
+    public void reset();
+
+    /**
+     * Start the timer running. Just set the running flag to true indicating
+     * that all time requests should be relative to the system clock.
+     */
+    public void start();
+
+    /**
+     * 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 void stop();
+
+
+    /**
+     * Check if the period specified has passed and if it has, advance the start
+     * time by that period. This is useful to decide if it's time to do periodic
+     * work without drifting later by the time it took to get around to
+     * checking.
+     *
+     * @param period The period to check for (in seconds).
+     * @return If the period has passed.
+     */
+    public boolean hasPeriodPassed(double period);
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/buttons/Button.java

@@ -0,0 +1,74 @@
+/*----------------------------------------------------------------------------*/
+/* 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 the command to start
+   */
+  public void toggleWhenPressed(final Command command) {
+    toggleWhenActive(command);
+  }
+
+  /**
+   * Cancel the command when the button is pressed
+   *$
+   * @param command the command to start
+   */
+  public void cancelWhenPressed(final Command command) {
+    cancelWhenActive(command);
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/buttons/InternalButton.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.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/src/shared/java/edu/wpi/first/wpilibj/buttons/JoystickButton.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.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/src/shared/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/src/shared/java/edu/wpi/first/wpilibj/buttons/Trigger.java

@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/command/Command.java

@@ -0,0 +1,591 @@
+/*----------------------------------------------------------------------------*/
+/* 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.RobotState;
+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() {
+    if (!m_runWhenDisabled && m_parent == null && RobotState.isDisabled()) {
+      cancel();
+    }
+    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/src/shared/java/edu/wpi/first/wpilibj/command/CommandGroup.java

@@ -0,0 +1,436 @@
+/*----------------------------------------------------------------------------*/
+/* 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) */
+  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 */
+  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/src/shared/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/command/LinkedListElement.java

@@ -0,0 +1,65 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/command/PIDCommand.java

@@ -0,0 +1,228 @@
+/*----------------------------------------------------------------------------*/
+/* 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.PIDSourceType;
+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 void setPIDSourceType(PIDSourceType pidSource) {}
+
+    public PIDSourceType getPIDSourceType() {
+      return PIDSourceType.kDisplacement;
+    }
+
+    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.
+   *
+   * @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#setInputRange(double, double) setInputRange(...)} 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#setInputRange(double, double) setInputRange(...)} 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();
+  }
+
+  /**
+   * Sets the maximum and minimum values expected from the input and setpoint.
+   *
+   * @param minimumInput the minimum value expected from the input and setpoint
+   * @param maximumInput the maximum value expected from the input and setpoint
+   */
+  protected void setInputRange(double minimumInput, double maximumInput) {
+    controller.setInputRange(minimumInput, maximumInput);
+  }
+
+  /**
+   * 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/src/shared/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java

@@ -0,0 +1,294 @@
+/*----------------------------------------------------------------------------*/
+/* 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.PIDSourceType;
+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 void setPIDSourceType(PIDSourceType pidSource) {}
+
+    public PIDSourceType getPIDSourceType() {
+      return PIDSourceType.kDisplacement;
+    }
+
+    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.
+   *
+   * @return the {@link PIDController} used by this {@link PIDSubsystem}
+   */
+  public PIDController getPIDController() {
+    return controller;
+  }
+
+
+  /**
+   * Adds the given value to the setpoint. If
+   * {@link PIDSubsystem#setInputRange(double, double) setInputRange(...)} 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 PIDSubsystem#setInputRange(double, double) setInputRange(...)} 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);
+  }
+
+  /**
+   * Sets the maximum and minimum values to write.
+   *
+   * @param minimumOutput the minimum value to write to the output
+   * @param maximumOutput the maximum value to write to the output
+   */
+  public void setOutputRange(double minimumOutput, double maximumOutput) {
+    controller.setOutputRange(minimumOutput, maximumOutput);
+  }
+
+  /**
+   * 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 the absolute tolerance
+   */
+  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 p the percent tolerance
+   */
+  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/src/shared/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/src/shared/java/edu/wpi/first/wpilibj/command/Scheduler.java

@@ -0,0 +1,375 @@
+/*----------------------------------------------------------------------------*/
+/* 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.HLUsageReporting;
+import edu.wpi.first.wpilibj.NamedSendable;
+import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
+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() {
+    HLUsageReporting.reportScheduler();
+  }
+
+  /**
+   * 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";
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public void initTable(ITable subtable) {
+    m_table = subtable;
+
+    m_table.putStringArray("Names", new String[0]);
+    m_table.putNumberArray("Ids", new double[0]);
+    m_table.putNumberArray("Cancel", new double[0]);
+  }
+
+  private void updateTable() {
+    if (m_table != null) {
+      // Get the commands to cancel
+      double[] toCancel = m_table.getNumberArray("Cancel", new double[0]);
+      if (toCancel.length > 0) {
+        for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
+          for (int i = 0; i < toCancel.length; i++) {
+            if (e.getData().hashCode() == toCancel[i]) {
+              e.getData().cancel();
+            }
+          }
+        }
+        m_table.putNumberArray("Cancel", new double[0]);
+      }
+
+      if (m_runningCommandsChanged) {
+        // Set the the running commands
+	int n = 0;
+        for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
+	  n++;
+        }
+        String[] commands = new String[n];
+        double[] ids = new double[n];
+	n = 0;
+        for (LinkedListElement e = firstCommand; e != null; e = e.getNext()) {
+          commands[n] = e.getData().getName();
+          ids[n] = e.getData().hashCode();
+	  n++;
+        }
+        m_table.putStringArray("Names", commands);
+        m_table.putNumberArray("Ids", ids);
+      }
+    }
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public ITable getTable() {
+    return m_table;
+  }
+
+  public String getSmartDashboardType() {
+    return "Scheduler";
+  }
+}

wpilibj/src/shared/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/src/shared/java/edu/wpi/first/wpilibj/command/StartCommand.java

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/command/Subsystem.java

@@ -0,0 +1,217 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/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/src/shared/java/edu/wpi/first/wpilibj/command/WaitForChildren.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.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 edu.wpi.first.wpilibj.Timer;
+
+/**
+ * WaitUntilCommand - waits until an absolute game time. This will wait until
+ * the game clock reaches some value, then continue to the next command.
+ *$
+ * @author brad
+ *
+ */
+public class WaitUntilCommand extends Command {
+
+  private double m_time;
+
+  public WaitUntilCommand(double time) {
+    super("WaitUntil(" + time + ")");
+    m_time = time;
+  }
+
+  public void initialize() {}
+
+  public void execute() {}
+
+  /**
+   * Check if we've reached the actual finish time.
+   */
+  public boolean isFinished() {
+    return Timer.getMatchTime() >= m_time;
+  }
+
+  public void end() {}
+
+  public void interrupted() {}
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java

@@ -0,0 +1,45 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for 3-axis accelerometers
+ */
+public interface Accelerometer {
+  public enum Range {
+    k2G, k4G, k8G, k16G
+  }
+
+  /**
+   * Common interface for setting the measuring range of an accelerometer.
+   *
+   * @param range The maximum acceleration, positive or negative, that the
+   *        accelerometer will measure. Not all accelerometers support all
+   *        ranges.
+   */
+  public void setRange(Range range);
+
+  /**
+   * Common interface for getting the x axis acceleration
+   *
+   * @return The acceleration along the x axis in g-forces
+   */
+  public double getX();
+
+  /**
+   * Common interface for getting the y axis acceleration
+   *
+   * @return The acceleration along the y axis in g-forces
+   */
+  public double getY();
+
+  /**
+   * Common interface for getting the z axis acceleration
+   *
+   * @return The acceleration along the z axis in g-forces
+   */
+  public double getZ();
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Gyro.java

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2014. 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 $(WIND_BASE)/WPILib. */
+/*----------------------------------------------------------------------------*/
+package edu.wpi.first.wpilibj.interfaces;
+
+/**
+ * Interface for yaw rate gyros
+ */
+public interface Gyro {
+  /**
+   * Initialize the gyro. Calibrate the gyro by running for a number of samples
+   * and computing the center value. 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.
+   */
+  public void initGyro();
+
+  /**
+   * 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();
+
+  /**
+   * 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 it will continue from 360 to 361 degrees. This
+   * allows algorithms that wouldn't want to see a discontinuity in the gyro
+   * output as it sweeps past from 360 to 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 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();
+
+  /**
+   * Free the resources used by the gyro
+   */
+  public void free();
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java

@@ -0,0 +1,17 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2014. 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.interfaces;
+
+import edu.wpi.first.wpilibj.PIDSource;
+
+/**
+ *
+ * @author alex
+ */
+public interface Potentiometer extends PIDSource {
+  double get();
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java

@@ -0,0 +1,222 @@
+/*
+ * 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;
+  private static ITable statusTable;
+  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");
+    livewindowTable = NetworkTable.getTable("LiveWindow");
+    statusTable = livewindowTable.getSubTable("~STATUS~");
+    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 type and
+   * channel like this: Gyro[1] for a gyro object connected to the first analog
+   * channel.
+   *
+   * @param moduleType A string indicating the type of the module used in the
+   *        naming (above)
+   * @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 channel, LiveWindowSendable component) {
+    addSensor("Ungrouped", moduleType + "[" + 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 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 channel, LiveWindowSendable component) {
+    addActuator("Ungrouped", moduleType + "[" + channel + "]", 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/src/shared/java/edu/wpi/first/wpilibj/livewindow/LiveWindowSendable.java

@@ -0,0 +1,30 @@
+/*
+ * 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/src/shared/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java

@@ -0,0 +1,148 @@
+/*----------------------------------------------------------------------------*/
+/* 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.tables.ITable;
+
+import java.util.ArrayList;
+
+/**
+ * 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 ArrayList<String> choices = new ArrayList<String>();
+  private ArrayList<Object> values = new ArrayList<Object>();
+  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.putStringArray(OPTIONS, choices.toArray(new String[0]));
+    }
+  }
+
+  /**
+   * 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.putStringArray(OPTIONS, choices.toArray(new String[0]));
+      if (defaultChoice != null) {
+        table.putString(DEFAULT, defaultChoice);
+      }
+    }
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public ITable getTable() {
+    return table;
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java

@@ -0,0 +1,329 @@
+/*----------------------------------------------------------------------------*/
+/* 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 edu.wpi.first.wpilibj.HLUsageReporting;
+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 {
+  /** 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();
+
+  static {
+    HLUsageReporting.reportSmartDashboard();
+  }
+
+  /**
+   * 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 NamedSendable} 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 key is null
+   */
+  public static Sendable getData(String key) {
+    ITable subtable = table.getSubTable(key);
+    Object data = tablesToData.get(subtable);
+    if (data == null) {
+      throw new IllegalArgumentException("SmartDashboard data does not exist: " + key);
+    } else {
+      return (Sendable) 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 NetworkTableKeyNotDefined 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 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, 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 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 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 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 getDouble(String key, double defaultValue) {
+    return table.getNumber(key, defaultValue);
+  }
+
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/util/AllocationException.java

@@ -0,0 +1,25 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/util/BaseSystemNotInitializedException.java

@@ -0,0 +1,39 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008-2014. 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;
+
+
+/**
+ * Thrown if there is an error caused by a basic system or setting not being
+ * properly initialized before being used.
+ *$
+ * @author Jonathan Leitschuh
+ */
+public class BaseSystemNotInitializedException extends RuntimeException {
+  /**
+   * Create a new BaseSystemNotInitializedException
+   *$
+   * @param message the message to attach to the exception
+   */
+  public BaseSystemNotInitializedException(String message) {
+    super(message);
+  }
+
+  /**
+   * Create a new BaseSystemNotInitializedException using the offending class
+   * that was not set and the class that was affected.
+   *$
+   * @param offender The class or interface that was not properly initialized.
+   * @param affected The class that was was affected by this missing
+   *        initialization.
+   */
+  public BaseSystemNotInitializedException(Class<?> offender, Class<?> affected) {
+    super("The " + offender.getSimpleName() + " for the " + affected.getSimpleName()
+        + " was never set.");
+  }
+
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/util/BoundaryException.java

@@ -0,0 +1,53 @@
+/*----------------------------------------------------------------------------*/
+/* 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 the message for a boundary exception
+   */
+  public static String getMessage(double value, double lower, double upper) {
+    return "Value must be between " + lower + " and " + upper + ", " + value + " given";
+  }
+}

wpilibj/src/shared/java/edu/wpi/first/wpilibj/util/CheckedAllocationException.java

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/util/SortedVector.java

@@ -0,0 +1,79 @@
+/*----------------------------------------------------------------------------*/
+/* 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/src/shared/java/edu/wpi/first/wpilibj/util/UncleanStatusException.java

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* 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;
+  }
+}