diff --git a/wpilibc/wpilibC++/include/Commands/PIDSubsystem.h b/wpilibc/wpilibC++/include/Commands/PIDSubsystem.h
index 065fa8e561..de04c3377d 100644
--- a/wpilibc/wpilibC++/include/Commands/PIDSubsystem.h
+++ b/wpilibc/wpilibC++/include/Commands/PIDSubsystem.h
@@ -52,6 +52,7 @@ class PIDSubsystem : public Subsystem, public PIDOutput, public PIDSource {
   void SetOutputRange(float minimumOutput, float maximumOutput);
   double GetSetpoint();
   double GetPosition();
+  double GetRate();
 
   virtual void SetAbsoluteTolerance(float absValue);
   virtual void SetPercentTolerance(float percent);
diff --git a/wpilibc/wpilibC++/include/PIDController.h b/wpilibc/wpilibC++/include/PIDController.h
index e5ab1ffe49..2994216d73 100644
--- a/wpilibc/wpilibC++/include/PIDController.h
+++ b/wpilibc/wpilibC++/include/PIDController.h
@@ -10,13 +10,16 @@
 #include "Controller.h"
 #include "LiveWindow/LiveWindow.h"
 #include "PIDInterface.h"
-#include "HAL/cpp/priority_mutex.h"
+#include "PIDSource.h"
 #include "Notifier.h"
+#include "HAL/cpp/priority_mutex.h"
 
 #include <memory>
 
+#include <atomic>
+#include <queue>
+
 class PIDOutput;
-class PIDSource;
 
 /**
  * Class implements a PID Control Loop.
@@ -53,10 +56,15 @@ class PIDController : public LiveWindowSendable,
   virtual double GetSetpoint() const override;
 
   virtual float GetError() const;
+  virtual float GetAvgError() const;
+
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+  virtual PIDSourceType GetPIDSourceType() const;
 
   virtual void SetTolerance(float percent);
   virtual void SetAbsoluteTolerance(float absValue);
   virtual void SetPercentTolerance(float percentValue);
+  virtual void SetToleranceBuffer(unsigned buf = 1);
   virtual bool OnTarget() const;
 
   virtual void Enable() override;
@@ -67,6 +75,13 @@ class PIDController : public LiveWindowSendable,
 
   virtual void InitTable(std::shared_ptr<ITable> table) override;
 
+ protected:
+  PIDSource *m_pidInput;
+  PIDOutput *m_pidOutput;
+
+  std::shared_ptr<ITable> m_table;
+  virtual void Calculate();
+
  private:
   float m_P;              // factor for "proportional" control
   float m_I;              // factor for "integral" control
@@ -86,17 +101,20 @@ class PIDController : public LiveWindowSendable,
     kPercentTolerance,
     kNoTolerance
   } m_toleranceType = kNoTolerance;
-  float m_tolerance = 0.05;  // the percetage or absolute error that is considered on
-                      // target
+
+  // the percetage or absolute error that is considered on target.
+  float m_tolerance = 0.05;
   float m_setpoint = 0;
   float m_error;
   float m_result = 0;
   float m_period;
 
-  mutable priority_mutex m_mutex;
+  // Length of buffer for averaging for tolerances.
+  std::atomic<unsigned> m_bufLength{1};
+  std::queue<double> m_buf;
+  double m_bufTotal = 0;
 
-  PIDSource *m_pidInput;
-  PIDOutput *m_pidOutput;
+  mutable priority_mutex m_mutex;
 
   std::unique_ptr<Notifier> m_controlLoop;
 
@@ -111,8 +129,4 @@ class PIDController : public LiveWindowSendable,
   virtual void UpdateTable() override;
   virtual void StartLiveWindowMode() override;
   virtual void StopLiveWindowMode() override;
-
- protected:
-  std::shared_ptr<ITable> m_table = nullptr;
-  virtual void Calculate();
 };
diff --git a/wpilibc/wpilibC++/include/PIDSource.h b/wpilibc/wpilibC++/include/PIDSource.h
index 6f5df76d0c..f0eb1cb08a 100644
--- a/wpilibc/wpilibC++/include/PIDSource.h
+++ b/wpilibc/wpilibC++/include/PIDSource.h
@@ -6,6 +6,8 @@
 /*----------------------------------------------------------------------------*/
 #pragma once
 
+enum class PIDSourceType { kDisplacement, kRate };
+
 /**
  * PIDSource interface is a generic sensor source for the PID class.
  * All sensors that can be used with the PID class will implement the PIDSource
@@ -14,6 +16,10 @@
  */
 class PIDSource {
  public:
-  enum PIDSourceParameter { kDistance, kRate, kAngle };
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+  PIDSourceType GetPIDSourceType() const;
   virtual double PIDGet() const = 0;
+
+ protected:
+  PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
 };
diff --git a/wpilibc/wpilibC++/include/interfaces/Potentiometer.h b/wpilibc/wpilibC++/include/interfaces/Potentiometer.h
index 66c2eb887f..d52892d0b3 100644
--- a/wpilibc/wpilibC++/include/interfaces/Potentiometer.h
+++ b/wpilibc/wpilibC++/include/interfaces/Potentiometer.h
@@ -16,12 +16,15 @@
 class Potentiometer : public PIDSource {
  public:
   virtual ~Potentiometer() = default;
+
   /**
    * Common interface for getting the current value of a potentiometer.
    *
    * @return The current set speed.  Value is between -1.0 and 1.0.
    */
   virtual double Get() const = 0;
+
+  virtual void SetPIDSourceType(PIDSourceType pidSource) override;
 };
 
 #endif
diff --git a/wpilibc/wpilibC++/src/Commands/PIDSubsystem.cpp b/wpilibc/wpilibC++/src/Commands/PIDSubsystem.cpp
index 7943b44140..f0219100a9 100644
--- a/wpilibc/wpilibC++/src/Commands/PIDSubsystem.cpp
+++ b/wpilibc/wpilibC++/src/Commands/PIDSubsystem.cpp
@@ -206,6 +206,12 @@ bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
  */
 double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
 
+/**
+ * Returns the current rate
+ * @return the current rate
+ */
+double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+
 void PIDSubsystem::PIDWrite(float output) { UsePIDOutput(output); }
 
 double PIDSubsystem::PIDGet() const { return ReturnPIDInput(); }
diff --git a/wpilibc/wpilibC++/src/PIDSource.cpp b/wpilibc/wpilibC++/src/PIDSource.cpp
new file mode 100644
index 0000000000..b6b5200704
--- /dev/null
+++ b/wpilibc/wpilibC++/src/PIDSource.cpp
@@ -0,0 +1,21 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. 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.  */
+/*----------------------------------------------------------------------------*/
+
+#include "PIDSource.h"
+
+/**
+ * Set which parameter you are using as a process control variable.
+ *
+ * @param pidSource An enum to select the parameter.
+ */
+void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidSource = pidSource;
+}
+
+PIDSourceType PIDSource::GetPIDSourceType() const {
+  return m_pidSource;
+}
diff --git a/wpilibc/wpilibC++/src/interfaces/Potentiometer.cpp b/wpilibc/wpilibC++/src/interfaces/Potentiometer.cpp
new file mode 100644
index 0000000000..38560907d1
--- /dev/null
+++ b/wpilibc/wpilibC++/src/interfaces/Potentiometer.cpp
@@ -0,0 +1,15 @@
+/*----------------------------------------------------------------------------*/
+/* 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 $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include <interfaces/Potentiometer.h>
+#include <Utility.h>
+
+void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) {
+  if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+    m_pidSource = pidSource;
+  }
+}
diff --git a/wpilibc/wpilibC++Devices/include/Encoder.h b/wpilibc/wpilibC++Devices/include/Encoder.h
index 77ab300626..7396eb0326 100644
--- a/wpilibc/wpilibC++Devices/include/Encoder.h
+++ b/wpilibc/wpilibC++Devices/include/Encoder.h
@@ -83,7 +83,6 @@ class Encoder : public SensorBase,
   void SetReverseDirection(bool reverseDirection);
   void SetSamplesToAverage(int samplesToAverage);
   int GetSamplesToAverage() const;
-  void SetPIDSourceParameter(PIDSourceParameter pidSource);
   double PIDGet() const override;
 
   void SetIndexSource(uint32_t channel, IndexingType type = kResetOnRisingEdge);
@@ -115,8 +114,6 @@ class Encoder : public SensorBase,
       nullptr;                     // Counter object for 1x and 2x encoding
   EncodingType m_encodingType;     // Encoding type
   int32_t m_encodingScale;         // 1x, 2x, or 4x, per the encodingType
-  PIDSourceParameter
-      m_pidSource = kDistance; // Encoder parameter that sources a PID controller
 
   std::shared_ptr<ITable> m_table = nullptr;
 };
diff --git a/wpilibc/wpilibC++Devices/include/Gyro.h b/wpilibc/wpilibC++Devices/include/Gyro.h
index ea20bc4dfa..df9434066e 100644
--- a/wpilibc/wpilibC++Devices/include/Gyro.h
+++ b/wpilibc/wpilibC++Devices/include/Gyro.h
@@ -49,7 +49,6 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
   virtual double GetRate() const;
   void SetSensitivity(float voltsPerDegreePerSecond);
   void SetDeadband(float volts);
-  void SetPIDSourceParameter(PIDSourceParameter pidSource);
   virtual void Reset();
   void InitGyro();
 
@@ -70,7 +69,6 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
   float m_voltsPerDegreePerSecond;
   float m_offset;
   uint32_t m_center;
-  PIDSourceParameter m_pidSource;
 
   std::shared_ptr<ITable> m_table = nullptr;
 };
diff --git a/wpilibc/wpilibC++Devices/include/Ultrasonic.h b/wpilibc/wpilibC++Devices/include/Ultrasonic.h
index eb58690031..a648f1d90c 100644
--- a/wpilibc/wpilibC++Devices/include/Ultrasonic.h
+++ b/wpilibc/wpilibC++Devices/include/Ultrasonic.h
@@ -70,6 +70,7 @@ class Ultrasonic : public SensorBase,
   void SetEnabled(bool enable) { m_enabled = enable; }
 
   double PIDGet() const override;
+  void SetPIDSourceType(PIDSourceType pidSource) override;
   void SetDistanceUnits(DistanceUnit units);
   DistanceUnit GetDistanceUnits() const;
 
diff --git a/wpilibc/wpilibC++Devices/src/Encoder.cpp b/wpilibc/wpilibC++Devices/src/Encoder.cpp
index 9a4b2a7f32..10dcb66358 100644
--- a/wpilibc/wpilibC++Devices/src/Encoder.cpp
+++ b/wpilibc/wpilibC++Devices/src/Encoder.cpp
@@ -475,17 +475,6 @@ int Encoder::GetSamplesToAverage() const {
   return result;
 }
 
-/**
- * Set which parameter of the encoder you are using as a process control
- * variable.
- *
- * @param pidSource An enum to select the parameter.
- */
-void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) {
-  if (StatusIsFatal()) return;
-  m_pidSource = pidSource;
-}
-
 /**
  * Implement the PIDSource interface.
  *
@@ -493,10 +482,10 @@ void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource) {
  */
 double Encoder::PIDGet() const {
   if (StatusIsFatal()) return 0.0;
-  switch (m_pidSource) {
-    case kDistance:
+  switch (GetPIDSourceType()) {
+    case PIDSourceType::kDisplacement:
       return GetDistance();
-    case kRate:
+    case PIDSourceType::kRate:
       return GetRate();
     default:
       return 0.0;
diff --git a/wpilibc/wpilibC++Devices/src/Gyro.cpp b/wpilibc/wpilibC++Devices/src/Gyro.cpp
index 504127b138..89c16834c8 100644
--- a/wpilibc/wpilibC++Devices/src/Gyro.cpp
+++ b/wpilibc/wpilibC++Devices/src/Gyro.cpp
@@ -62,7 +62,7 @@ void Gyro::InitGyro() {
 
   SetDeadband(0.0f);
 
-  SetPIDSourceParameter(kAngle);
+  SetPIDSourceType(PIDSourceType::kDisplacement);
 
   HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
   LiveWindow::GetInstance().AddSensor("Gyro", m_analog->GetChannel(), this);
@@ -189,27 +189,17 @@ void Gyro::SetDeadband(float volts) {
   m_analog->SetAccumulatorDeadband(deadband);
 }
 
-/**
- * Sets the type of output to use with the WPILib PID class
- * The gyro supports using either rate or angle for PID calculations
- */
-void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource) {
-  if (pidSource == 0 || pidSource > 2)
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
-  m_pidSource = pidSource;
-}
-
 /**
  * Get the PIDOutput for the PIDSource base object. Can be set to return
- * angle or rate using SetPIDSourceParameter(). Defaults to angle.
+ * angle or rate using SetPIDSourceType(). Defaults to angle.
  *
  * @return The PIDOutput (angle or rate, defaults to angle)
  */
 double Gyro::PIDGet() const {
-  switch (m_pidSource) {
-    case kRate:
+  switch (GetPIDSourceType()) {
+    case PIDSourceType::kRate:
       return GetRate();
-    case kAngle:
+    case PIDSourceType::kDisplacement:
       return GetAngle();
     default:
       return 0;
diff --git a/wpilibc/wpilibC++Devices/src/PIDController.cpp b/wpilibc/wpilibC++Devices/src/PIDController.cpp
index e80eacd59e..6fb55f1484 100644
--- a/wpilibc/wpilibC++Devices/src/PIDController.cpp
+++ b/wpilibc/wpilibC++Devices/src/PIDController.cpp
@@ -127,20 +127,37 @@ void PIDController::Calculate() {
         }
       }
 
-      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;
+      if (m_pidInput->GetPIDSourceType() == 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_error + m_I * m_totalError +
-                 m_D * (m_prevInput - input) + m_setpoint * m_F;
+        m_result = m_D * m_error + m_P * m_totalError + 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)
@@ -152,6 +169,15 @@ void PIDController::Calculate() {
       result = m_result;
 
       pidOutput->PIDWrite(result);
+
+      // Update the buffer.
+      m_buf.push(m_error);
+      m_bufTotal += m_error;
+      // Remove old elements when buffer is full.
+      if (m_buf.size() > m_bufLength) {
+        m_bufTotal -= m_buf.front();
+        m_buf.pop();
+      }
     }
   }
 }
@@ -293,6 +319,7 @@ void PIDController::SetOutputRange(float minimumOutput, float maximumOutput) {
 
 /**
  * Set the setpoint for the PIDController
+ * Clears the queue for GetAvgError().
  * @param setpoint the desired setpoint
  */
 void PIDController::SetSetpoint(float setpoint) {
@@ -308,6 +335,9 @@ void PIDController::SetSetpoint(float setpoint) {
     } else {
       m_setpoint = setpoint;
     }
+
+    // Clear m_buf.
+    m_buf = std::queue<double>();
   }
 
   if (m_table != nullptr) {
@@ -325,7 +355,7 @@ double PIDController::GetSetpoint() const {
 }
 
 /**
- * Retruns the current difference of the input from the setpoint
+ * Returns the current difference of the input from the setpoint
  * @return the current error
  */
 float PIDController::GetError() const {
@@ -337,6 +367,36 @@ float PIDController::GetError() const {
   return GetSetpoint() - pidInput;
 }
 
+/**
+ * Sets what type of input the PID controller will use
+ */
+void PIDController::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidInput->SetPIDSourceType(pidSource);
+}
+/**
+ * Returns the type of input the PID controller is using
+ * @return the PID controller input type
+ */
+PIDSourceType PIDController::GetPIDSourceType() const {
+  return m_pidInput->GetPIDSourceType();
+}
+
+/**
+ * Returns the current average of the error over the past few iterations.
+ * You can specify the number of iterations to average with SetToleranceBuffer()
+ * (defaults to 1). This is the same value that is used for OnTarget().
+ * @return the average error
+ */
+float PIDController::GetAvgError() const {
+  float avgError = 0;
+  {
+    std::unique_lock<priority_mutex> sync(m_mutex);
+    // Don't divide by zero.
+    if (m_buf.size()) avgError = m_bufTotal / m_buf.size();
+  }
+  return avgError;
+}
+
 /*
  * Set the percentage error which is considered tolerable for use with
  * OnTarget.
@@ -376,6 +436,25 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
   }
 }
 
+/*
+ * 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. Defaults to 1.
+ */
+void PIDController::SetToleranceBuffer(unsigned bufLength) {
+  m_bufLength = bufLength;
+
+  // Cut the buffer down to size if needed.
+  while (m_buf.size() > bufLength) {
+    m_bufTotal -= m_buf.front();
+    m_buf.pop();
+  }
+}
+
 /*
  * Return true if the error is within the percentage of the total input range,
  * determined by SetTolerance. This asssumes that the maximum and minimum input
@@ -386,7 +465,7 @@ void PIDController::SetAbsoluteTolerance(float absTolerance) {
  * time.
  */
 bool PIDController::OnTarget() const {
-  double error = GetError();
+  double error = GetAvgError();
 
   std::unique_lock<priority_mutex> sync(m_mutex);
   switch (m_toleranceType) {
diff --git a/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp b/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
index 37d4afe0fe..e81b990fc1 100644
--- a/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
+++ b/wpilibc/wpilibC++Devices/src/Ultrasonic.cpp
@@ -316,6 +316,12 @@ double Ultrasonic::PIDGet() const {
   }
 }
 
+void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
+  if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
+    m_pidSource = pidSource;
+  }
+}
+
 /**
  * Set the current DistanceUnit that should be used for the PIDSource base
  * object.
diff --git a/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp b/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp
index e736dfc8ef..1175c2dc4b 100644
--- a/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp
+++ b/wpilibc/wpilibC++IntegrationTests/src/MotorEncoderTest.cpp
@@ -122,11 +122,12 @@ TEST_P(MotorEncoderTest, ClampSpeed) {
 }
 
 /**
- * Test if PID loops work
+ * Test if position PID loop works
  */
-TEST_P(MotorEncoderTest, PIDController) {
+TEST_P(MotorEncoderTest, PositionPIDController) {
   Reset();
 
+  m_encoder->SetPIDSourceType(PIDSourceType::kDisplacement);
   PIDController pid(0.001f, 0.0005f, 0.0f, m_encoder, m_speedController);
   pid.SetAbsoluteTolerance(20.0f);
   pid.SetOutputRange(-0.3f, 0.3f);
@@ -142,6 +143,30 @@ TEST_P(MotorEncoderTest, PIDController) {
   EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds.";
 }
 
+/**
+ * Test if velocity PID loop works
+ */
+TEST_P(MotorEncoderTest, VelocityPIDController) {
+  Reset();
+
+  m_encoder->SetPIDSourceType(PIDSourceType::kRate);
+  PIDController pid(1e-5, 0.0f, 3e-5, 8e-5, m_encoder, m_speedController);
+  pid.SetAbsoluteTolerance(50.0f);
+  pid.SetToleranceBuffer(10);
+  pid.SetOutputRange(-0.3f, 0.3f);
+  pid.SetSetpoint(2000);
+
+  /* 10 seconds should be plenty time to get to the setpoint */
+  pid.Enable();
+  Wait(10.0);
+
+  RecordProperty("PIDError", pid.GetAvgError());
+
+  EXPECT_TRUE(pid.OnTarget()) << "PID loop did not converge within 10 seconds. Goal was: " << 2000 << " Error was: " << pid.GetError();
+
+  pid.Disable();
+}
+
 /**
  * Test resetting encoders
  */
diff --git a/wpilibc/wpilibC++Sim/include/Encoder.h b/wpilibc/wpilibC++Sim/include/Encoder.h
index d024a4a18f..b573522a1f 100644
--- a/wpilibc/wpilibC++Sim/include/Encoder.h
+++ b/wpilibc/wpilibC++Sim/include/Encoder.h
@@ -53,7 +53,7 @@ public:
 	void SetReverseDirection(bool reverseDirection);
 	void SetSamplesToAverage(int samplesToAverage);
 	int GetSamplesToAverage() const;
-	void SetPIDSourceParameter(PIDSourceParameter pidSource);
+	void SetPIDSourceType(PIDSourceType pidSource);
 	double PIDGet() const override;
 
 	void UpdateTable() override;
@@ -80,7 +80,6 @@ private:
 	double m_distancePerPulse;		// distance of travel for each encoder tick
 	EncodingType m_encodingType;	// Encoding type
 	int32_t m_encodingScale;		// 1x, 2x, or 4x, per the encodingType
-	PIDSourceParameter m_pidSource;	// Encoder parameter that sources a PID controller
 	bool m_reverseDirection;
 	SimEncoder* impl;
 
diff --git a/wpilibc/wpilibC++Sim/include/Gyro.h b/wpilibc/wpilibC++Sim/include/Gyro.h
index 1204a809c1..598a313244 100644
--- a/wpilibc/wpilibC++Sim/include/Gyro.h
+++ b/wpilibc/wpilibC++Sim/include/Gyro.h
@@ -38,10 +38,10 @@ public:
 	virtual ~Gyro() = default;
 	virtual float GetAngle() const;
 	virtual double GetRate() const;
-	void SetPIDSourceParameter(PIDSourceParameter pidSource);
 	virtual void Reset();
 
 	// PIDSource interface
+	void SetPIDSourceType(PIDSourceType pidSource) override;
 	double PIDGet() const override;
 
 	void UpdateTable() override;
@@ -55,7 +55,6 @@ private:
 	void InitGyro(int channel);
 
 	SimGyro* impl;
-	PIDSourceParameter m_pidSource;
 
 	std::shared_ptr<ITable> m_table = nullptr;
 };
diff --git a/wpilibc/wpilibC++Sim/src/Encoder.cpp b/wpilibc/wpilibC++Sim/src/Encoder.cpp
index f4c1436e08..ecd658bd1c 100644
--- a/wpilibc/wpilibC++Sim/src/Encoder.cpp
+++ b/wpilibc/wpilibC++Sim/src/Encoder.cpp
@@ -34,7 +34,6 @@ void Encoder::InitEncoder(int channelA, int channelB, bool reverseDirection, Enc
 
 	int32_t index = 0;
 	m_distancePerPulse = 1.0;
-	m_pidSource = kDistance;
 
 	LiveWindow::GetInstance().AddSensor("Encoder", channelA, this);
 
@@ -309,7 +308,7 @@ void Encoder::SetReverseDirection(bool reverseDirection)
  *
  * @param pidSource An enum to select the parameter.
  */
-void Encoder::SetPIDSourceParameter(PIDSourceParameter pidSource)
+void Encoder::SetPIDSourceType(PIDSourceType pidSource)
 {
 	m_pidSource = pidSource;
 }
@@ -323,9 +322,9 @@ double Encoder::PIDGet() const
 {
 	switch (m_pidSource)
 	{
-	case kDistance:
+	case PIDSourceType::kDisplacement:
 		return GetDistance();
-	case kRate:
+	case PIDSourceType::kRate:
 		return GetRate();
 	default:
 		return 0.0;
diff --git a/wpilibc/wpilibC++Sim/src/Gyro.cpp b/wpilibc/wpilibC++Sim/src/Gyro.cpp
index cf0e6f3adb..bd4bba6ede 100644
--- a/wpilibc/wpilibC++Sim/src/Gyro.cpp
+++ b/wpilibc/wpilibC++Sim/src/Gyro.cpp
@@ -25,7 +25,7 @@ constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
  */
 void Gyro::InitGyro(int channel)
 {
-	SetPIDSourceParameter(kAngle);
+	SetPIDSourceType(PIDSourceType::kDisplacement);
 
 	char buffer[50];
 	int n = sprintf(buffer, "analog/%d", channel);
@@ -83,10 +83,8 @@ double Gyro::GetRate() const
     return impl->GetVelocity();
 }
 
-void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
+void Gyro::SetPIDSourceType(PIDSourceType pidSource)
 {
-	if(pidSource == 0 || pidSource > 2)
-		wpi_setWPIErrorWithContext(ParameterOutOfRange, "Gyro pidSource");
     m_pidSource = pidSource;
 }
 
@@ -97,10 +95,10 @@ void Gyro::SetPIDSourceParameter(PIDSourceParameter pidSource)
  */
 double Gyro::PIDGet() const
 {
-	switch(m_pidSource){
-	case kRate:
+	switch(GetPIDSourceType()){
+	case PIDSourceType::kRate:
 		return GetRate();
-	case kAngle:
+	case PIDSourceType::kDisplacement:
 		return GetAngle();
 	default:
 		return 0;
diff --git a/wpilibc/wpilibC++Sim/src/PIDController.cpp b/wpilibc/wpilibC++Sim/src/PIDController.cpp
index a180a835e5..1b1f6d198c 100644
--- a/wpilibc/wpilibC++Sim/src/PIDController.cpp
+++ b/wpilibc/wpilibC++Sim/src/PIDController.cpp
@@ -148,23 +148,42 @@ void PIDController::Calculate()
 				}
 			}
 
-			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;
-				}
-			}
+            if (m_pidInput->GetPIDSourceType() == 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_error + m_I * m_totalError + m_D * (m_prevInput - input) + m_setpoint * m_F;
+              m_result = m_D * m_error + m_P * m_totalError + 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;
diff --git a/wpilibj/enum.txt b/wpilibj/enum.txt
deleted file mode 100644
index 9f85ed1bcf..0000000000
--- a/wpilibj/enum.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-//Going to use this pattern for enumeration classes, since they are otherwise unsupported
-
-//The typesafe enum pattern
-public class Suit {
-    public final int value;
-
-    protected static final int CLUBS_VAL = 0;
-    protected static final int DIAMONDS_VAL = 1;
-    protected static final int HEARTS_VAL = 2;
-    protected static final int SPADES_VAL = 3;
-
-    public static final Suit CLUBS = new Suit(CLUBS_VAL);
-    public static final Suit DIAMONDS = new Suit(DIAMONDS_VAL);
-    public static final Suit HEARTS = new Suit(HEARTS_VAL);
-    public static final Suit SPADES = new Suit(SPADES_VAL);
-
-    private Suit(int value){
-        this.value = value;
-    }
-}
diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java
index 139c4d26c0..8658110ffb 100644
--- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -6,6 +6,7 @@
 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;
@@ -39,12 +40,15 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
                                      // 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;
-  PIDSource m_pidInput;
-  PIDOutput m_pidOutput;
+  protected PIDSource m_pidInput;
+  protected PIDOutput m_pidOutput;
   java.util.Timer m_controlLoop;
   private boolean m_freed = false;
   private boolean m_usingPercentTolerance;
@@ -79,7 +83,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return (Math.abs(getError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
+      return (Math.abs(getAvgError()) < percentage / 100 * (m_maximumInput - m_minimumInput));
     }
   }
 
@@ -92,7 +96,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
     @Override
     public boolean onTarget() {
-      return Math.abs(getError()) < value;
+      return Math.abs(getAvgError()) < value;
     }
   }
 
@@ -153,6 +157,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     instances++;
     HLUsageReporting.reportPIDController(instances);
     m_tolerance = new NullTolerance();
+
+    m_buf = new LinkedList<Double>();
   }
 
   /**
@@ -256,21 +262,39 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
           }
         }
 
-        if (m_I != 0) {
-          double potentialIGain = (m_totalError + m_error) * m_I;
-          if (potentialIGain < m_maximumOutput) {
-            if (potentialIGain > m_minimumOutput) {
-              m_totalError += m_error;
+        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_minimumOutput / m_I;
+              m_totalError = m_maximumOutput / m_P;
             }
-          } else {
-            m_totalError = m_maximumOutput / m_I;
+
+            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_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) {
@@ -280,6 +304,14 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
         }
         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);
@@ -427,6 +459,7 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
 
   /**
    * Set the setpoint for the PIDController
+   * Clears the queue for GetAvgError().
    *$
    * @param setpoint the desired setpoint
    */
@@ -443,6 +476,8 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
       m_setpoint = setpoint;
     }
 
+    m_buf.clear();
+
     if (table != null)
       table.putNumber("setpoint", m_setpoint);
   }
@@ -466,6 +501,39 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     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)
@@ -513,6 +581,24 @@ public class PIDController implements PIDInterface, LiveWindowSendable, Controll
     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
diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
index 60b7457e0d..839ae4bae1 100644
--- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
+++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -6,6 +6,7 @@
 /*----------------------------------------------------------------------------*/
 
 package edu.wpi.first.wpilibj;
+import edu.wpi.first.wpilibj.PIDSourceType;
 
 /**
  * This interface allows for PIDController to automatically read from this
@@ -14,23 +15,22 @@ package edu.wpi.first.wpilibj;
  * @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);
 
   /**
-   * A description for the type of output value to provide to a PIDController
+   * Get which parameter of the device you are using as a process control
+   * variable.
+   *
+   * @return the currently selected PID source parameter
    */
-  public static class PIDSourceParameter {
-    public final int value;
-    static final int kDistance_val = 0;
-    static final int kRate_val = 1;
-    static final int kAngle_val = 2;
-    public static final PIDSourceParameter kDistance = new PIDSourceParameter(kDistance_val);
-    public static final PIDSourceParameter kRate = new PIDSourceParameter(kRate_val);
-    public static final PIDSourceParameter kAngle = new PIDSourceParameter(kAngle_val);
-
-    private PIDSourceParameter(int value) {
-      this.value = value;
-    }
-  }
+  public PIDSourceType getPIDSourceType();
 
   /**
    * Get the result to use in PIDController
diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
new file mode 100644
index 0000000000..fddb11f022
--- /dev/null
+++ b/wpilibj/wpilibJava/src/main/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;
+  }
+}
diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
index c05ec02b5f..7eff7c5078 100644
--- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
+++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
@@ -10,6 +10,7 @@ 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;
 
@@ -37,6 +38,11 @@ public abstract class PIDCommand extends Command implements Sendable {
   };
   /** 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();
diff --git a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
index 2f9f097d0a..5dc392a35a 100644
--- a/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
+++ b/wpilibj/wpilibJava/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -10,6 +10,7 @@ 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;
 
@@ -39,6 +40,11 @@ public abstract class PIDSubsystem extends Subsystem implements Sendable {
   };
   /** 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();
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
index e8736dae5e..174d4616cd 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -30,18 +30,15 @@ public class ADXL345_I2C extends SensorBase implements Accelerometer, LiveWindow
   private static final byte kDataFormat_SelfTest = (byte) 0x80, kDataFormat_SPI = 0x40,
       kDataFormat_IntInvert = 0x20, kDataFormat_FullRes = 0x08, kDataFormat_Justify = 0x04;
 
-  public static class Axes {
+  public static enum Axes {
+    kX((byte) 0x00),
+    kY((byte) 0x02),
+    kZ((byte) 0x04);
 
     /**
      * The integer value representing this enumeration
      */
     public final byte value;
-    static final byte kX_val = 0x00;
-    static final byte kY_val = 0x02;
-    static final byte kZ_val = 0x04;
-    public static final Axes kX = new Axes(kX_val);
-    public static final Axes kY = new Axes(kY_val);
-    public static final Axes kZ = new Axes(kZ_val);
 
     private Axes(byte value) {
       this.value = value;
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
index 2ce7053c17..561102e55d 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -42,18 +42,15 @@ public class ADXL345_SPI extends SensorBase implements Accelerometer, LiveWindow
   private static final int kDataFormat_FullRes = 0x08;
   private static final int kDataFormat_Justify = 0x04;
 
-  public static class Axes {
+  public static enum Axes {
+    kX((byte) 0x00),
+    kY((byte) 0x02),
+    kZ((byte) 0x04);
 
     /**
      * The integer value representing this enumeration
      */
     public final byte value;
-    static final byte kX_val = 0x00;
-    static final byte kY_val = 0x02;
-    static final byte kZ_val = 0x04;
-    public static final ADXL345_SPI.Axes kX = new ADXL345_SPI.Axes(kX_val);
-    public static final ADXL345_SPI.Axes kY = new ADXL345_SPI.Axes(kY_val);
-    public static final ADXL345_SPI.Axes kZ = new ADXL345_SPI.Axes(kZ_val);
 
     private Axes(byte value) {
       this.value = value;
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
index 15ee7c2675..3a4a6914d9 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -26,6 +26,7 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi
   private double m_voltsPerG = 1.0;
   private double m_zeroGVoltage = 2.5;
   private boolean m_allocatedChannel;
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * Common initialization
@@ -112,6 +113,20 @@ public class AnalogAccelerometer extends SensorBase implements PIDSource, LiveWi
     m_zeroGVoltage = zero;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Get the Acceleration for the PID Source parent.
    *
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index 3c5ccab338..d496a29112 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -46,6 +46,7 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend
   private int m_channel;
   private static final int[] kAccumulatorChannels = {0, 1};
   private long m_accumulatorOffset;
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * Construct an analog channel.
@@ -460,6 +461,20 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend
     return value;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Get the average voltage for use with PIDController
    *
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
index 2eeac0e91c..9b2107f3db 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -28,6 +28,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
   private double m_fullRange, m_offset;
   private AnalogInput m_analog_input;
   private boolean m_init_analog_input;
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * Common initialization code called by all constructors.
@@ -152,6 +153,23 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
     return (m_analog_input.getVoltage() / ControllerPower.getVoltage5V()) * m_fullRange + m_offset;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+      throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
+    }
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Implement the PIDSource interface.
    *
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
index aa5bd56c59..0fc1190f00 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CANTalon.java
@@ -16,6 +16,7 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface
     CANSpeedController, LiveWindowSendable {
   private MotorSafetyHelper m_safetyHelper;
   private boolean isInverted = false;
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   public enum TalonControlMode {
     PercentVbus(0), Follower(5), Voltage(4), Position(1), Speed(2), Current(3), Disabled(15);
@@ -123,6 +124,20 @@ public class CANTalon implements MotorSafety, PIDOutput, PIDSource, PIDInterface
     }
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   @Override
   public double pidGet() {
     return getPosition();
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java
index 28c3057cba..06e1787b94 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -67,7 +67,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
   private boolean m_allocatedDownSource;
   private ByteBuffer m_counter; // /< The FPGA counter object.
   private int m_index; // /< The index of this counter.
-  private PIDSourceParameter m_pidSource;
+  private PIDSourceType m_pidSource;
   private double m_distancePerPulse; // distance of travel for each tick
 
   private void initCounter(final Mode mode) {
@@ -640,7 +640,7 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
    *
    * @param pidSource An enum to select the parameter.
    */
-  public void setPIDSourceParameter(PIDSourceParameter pidSource) {
+  public void setPIDSourceType(PIDSourceType pidSource) {
     if (pidSource == null) {
       throw new NullPointerException("PID Source Parameter given was null");
     }
@@ -648,12 +648,19 @@ public class Counter extends SensorBase implements CounterBase, LiveWindowSendab
     m_pidSource = pidSource;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   @Override
   public double pidGet() {
-    switch (m_pidSource.value) {
-      case PIDSourceParameter.kDistance_val:
+    switch (m_pidSource) {
+      case kDisplacement:
         return getDistance();
-      case PIDSourceParameter.kRate_val:
+      case kRate:
         return getRate();
       default:
         return 0.0;
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CounterBase.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
index be26bacb91..e4eb915e2b 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
@@ -20,27 +20,24 @@ public interface CounterBase {
   /**
    * The number of edges for the counterbase to increment or decrement on
    */
-  public static class EncodingType {
+  public enum EncodingType {
+    /**
+     * Count only the rising edge
+     */
+    k1X(0),
+    /**
+     * Count both the rising and falling edge
+     */
+    k2X(1),
+    /**
+     * Count rising and falling on both channels
+     */
+    k4X(2);
 
     /**
      * The integer value representing this enumeration
      */
     public final int value;
-    static final int k1X_val = 0;
-    static final int k2X_val = 1;
-    static final int k4X_val = 2;
-    /**
-     * Count only the rising edge
-     */
-    public static final EncodingType k1X = new EncodingType(k1X_val);
-    /**
-     * Count both the rising and falling edge
-     */
-    public static final EncodingType k2X = new EncodingType(k2X_val);
-    /**
-     * Count rising and falling on both channels
-     */
-    public static final EncodingType k4X = new EncodingType(k4X_val);
 
     private EncodingType(int value) {
       this.value = value;
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
index 7527f00b8a..c5045e7523 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -26,19 +26,10 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
   /**
    * Possible values for a DoubleSolenoid
    */
-  public static class Value {
-
-    public final int value;
-    public static final int kOff_val = 0;
-    public static final int kForward_val = 1;
-    public static final int kReverse_val = 2;
-    public static final Value kOff = new Value(kOff_val);
-    public static final Value kForward = new Value(kForward_val);
-    public static final Value kReverse = new Value(kReverse_val);
-
-    private Value(int value) {
-      this.value = value;
-    }
+  public static enum Value {
+    kOff,
+    kForward,
+    kReverse
   }
 
   private int m_forwardChannel; // /< The forward channel on the module to
@@ -119,14 +110,14 @@ public class DoubleSolenoid extends SolenoidBase implements LiveWindowSendable {
   public void set(final Value value) {
     byte rawValue = 0;
 
-    switch (value.value) {
-      case Value.kOff_val:
+    switch (value) {
+      case kOff:
         rawValue = 0x00;
         break;
-      case Value.kForward_val:
+      case kForward:
         rawValue = m_forwardMask;
         break;
-      case Value.kReverse_val:
+      case kReverse:
         rawValue = m_reverseMask;
         break;
     }
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java
index a878c7ece8..269bea15a8 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -59,7 +59,7 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
   private boolean m_allocatedA;
   private boolean m_allocatedB;
   private boolean m_allocatedI;
-  private PIDSourceParameter m_pidSource;
+  private PIDSourceType m_pidSource;
 
   /**
    * Common initialization code for Encoders. This code allocates resources for
@@ -77,8 +77,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    *        exactly match the spec'd count or be double (2x) the spec'd count.
    */
   private void initEncoder(boolean reverseDirection) {
-    switch (m_encodingType.value) {
-      case EncodingType.k4X_val:
+    switch (m_encodingType) {
+      case k4X:
         m_encodingScale = 4;
         ByteBuffer status = ByteBuffer.allocateDirect(4);
         // set the byte order
@@ -97,15 +97,15 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
         m_counter = null;
         setMaxPeriod(.5);
         break;
-      case EncodingType.k2X_val:
-      case EncodingType.k1X_val:
+      case k2X:
+      case k1X:
         m_encodingScale = m_encodingType == EncodingType.k1X ? 1 : 2;
         m_counter = new Counter(m_encodingType, m_aSource, m_bSource, reverseDirection);
         m_index = m_counter.getFPGAIndex();
         break;
     }
     m_distancePerPulse = 1.0;
-    m_pidSource = PIDSourceParameter.kDistance;
+    m_pidSource = PIDSourceType.kDisplacement;
 
     UsageReporting.report(tResourceType.kResourceType_Encoder, m_index, m_encodingType.value);
     LiveWindow.addSensor("Encoder", m_aSource.getChannelForRouting(), this);
@@ -526,12 +526,12 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    * pulses.
    */
   private double decodingScaleFactor() {
-    switch (m_encodingType.value) {
-      case EncodingType.k1X_val:
+    switch (m_encodingType) {
+      case k1X:
         return 1.0;
-      case EncodingType.k2X_val:
+      case k2X:
         return 0.5;
-      case EncodingType.k4X_val:
+      case k4X:
         return 0.25;
       default:
         // This is never reached, EncodingType enum limits values
@@ -611,8 +611,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    * @param samplesToAverage The number of samples to average from 1 to 127.
    */
   public void setSamplesToAverage(int samplesToAverage) {
-    switch (m_encodingType.value) {
-      case EncodingType.k4X_val:
+    switch (m_encodingType) {
+      case k4X:
         ByteBuffer status = ByteBuffer.allocateDirect(4);
         // set the byte order
         status.order(ByteOrder.LITTLE_ENDIAN);
@@ -622,8 +622,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
         }
         HALUtil.checkStatus(status.asIntBuffer());
         break;
-      case EncodingType.k1X_val:
-      case EncodingType.k2X_val:
+      case k1X:
+      case k2X:
         m_counter.setSamplesToAverage(samplesToAverage);
     }
   }
@@ -637,16 +637,16 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    *         127)
    */
   public int getSamplesToAverage() {
-    switch (m_encodingType.value) {
-      case EncodingType.k4X_val:
+    switch (m_encodingType) {
+      case k4X:
         ByteBuffer status = ByteBuffer.allocateDirect(4);
         // set the byte order
         status.order(ByteOrder.LITTLE_ENDIAN);
         int value = EncoderJNI.getEncoderSamplesToAverage(m_encoder, status.asIntBuffer());
         HALUtil.checkStatus(status.asIntBuffer());
         return value;
-      case EncodingType.k1X_val:
-      case EncodingType.k2X_val:
+      case k1X:
+      case k2X:
         return m_counter.getSamplesToAverage();
     }
     return 1;
@@ -658,21 +658,27 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    *
    * @param pidSource An enum to select the parameter.
    */
-  public void setPIDSourceParameter(PIDSourceParameter pidSource) {
-    BoundaryException.assertWithinBounds(pidSource.value, 0, 1);
+  public void setPIDSourceType(PIDSourceType pidSource) {
     m_pidSource = pidSource;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Implement the PIDSource interface.
    *
    * @return The current value of the selected source parameter.
    */
   public double pidGet() {
-    switch (m_pidSource.value) {
-      case PIDSourceParameter.kDistance_val:
+    switch (m_pidSource) {
+      case kDisplacement:
         return getDistance();
-      case PIDSourceParameter.kRate_val:
+      case kRate:
         return getRate();
       default:
         return 0.0;
@@ -745,8 +751,8 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
    * Live Window code, only does anything if live window is activated.
    */
   public String getSmartDashboardType() {
-    switch (m_encodingType.value) {
-      case EncodingType.k4X_val:
+    switch (m_encodingType) {
+      case k4X:
         return "Quadrature Encoder";
       default:
         return "Encoder";
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java
index 3b83e32a2a..0e756a952d 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Gyro.java
@@ -11,7 +11,6 @@ import edu.wpi.first.wpilibj.communication.UsageReporting;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
 import edu.wpi.first.wpilibj.tables.ITable;
-import edu.wpi.first.wpilibj.util.BoundaryException;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position.
@@ -35,7 +34,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
   int m_center;
   boolean m_channelAllocated = false;
   AccumulatorResult result;
-  private PIDSourceParameter m_pidSource;
+  private PIDSourceType m_pidSource;
 
   /**
    * Initialize the gyro. Calibrate the gyro by running for a number of samples
@@ -73,7 +72,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
 
     setDeadband(0.0);
 
-    setPIDSourceParameter(PIDSourceParameter.kAngle);
+    setPIDSourceType(PIDSourceType.kDisplacement);
 
     UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
     LiveWindow.addSensor("Gyro", m_analog.getChannel(), this);
@@ -199,27 +198,33 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
 
   /**
    * Set which parameter of the gyro you are using as a process control
-   * variable. The Gyro class supports the rate and angle parameters
+   * variable. The Gyro class supports the rate and displacement parameters
    *
    * @param pidSource An enum to select the parameter.
    */
-  public void setPIDSourceParameter(PIDSourceParameter pidSource) {
-    BoundaryException.assertWithinBounds(pidSource.value, 1, 2);
+  public void setPIDSourceType(PIDSourceType pidSource) {
     m_pidSource = pidSource;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Get the output of the gyro for use with PIDControllers. May be the angle or
-   * rate depending on the set PIDSourceParameter
+   * rate depending on the set PIDSourceType
    *
    * @return the output according to the gyro
    */
   @Override
   public double pidGet() {
-    switch (m_pidSource.value) {
-      case PIDSourceParameter.kRate_val:
+    switch (m_pidSource) {
+      case kRate:
         return getRate();
-      case PIDSourceParameter.kAngle_val:
+      case kDisplacement:
         return getAngle();
       default:
         return 0.0;
diff --git a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
index 213b829974..898ceb2608 100644
--- a/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
+++ b/wpilibj/wpilibJavaDevices/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -29,26 +29,15 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
   /**
    * The units to return when PIDGet is called
    */
-  public static class Unit {
-
-    /**
-     * The integer value representing this enumeration
-     */
-    public final int value;
-    static final int kInches_val = 0;
-    static final int kMillimeters_val = 1;
+  public static enum Unit {
     /**
      * Use inches for PIDGet
      */
-    public static final Unit kInches = new Unit(kInches_val);
+    kInches,
     /**
      * Use millimeters for PIDGet
      */
-    public static final Unit kMillimeter = new Unit(kMillimeters_val);
-
-    private Unit(int value) {
-      this.value = value;
-    }
+    kMillimeters
   }
 
   private static final double kPingTime = 10 * 1e-6; // /< Time (sec) for the
@@ -72,6 +61,7 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
   // sensing
   private Unit m_units;
   private static int m_instances = 0;
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * Background task that goes through the list of ultrasonic sensors and pings
@@ -344,16 +334,33 @@ public class Ultrasonic extends SensorBase implements PIDSource, LiveWindowSenda
     return getRangeInches() * 25.4;
   }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+      throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
+    }
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
   /**
    * Get the range in the current DistanceUnit for the PIDSource base object.
    *
    * @return The range in DistanceUnit
    */
   public double pidGet() {
-    switch (m_units.value) {
-      case Unit.kInches_val:
+    switch (m_units) {
+      case kInches:
         return getRangeInches();
-      case Unit.kMillimeters_val:
+      case kMillimeters:
         return getRangeMM();
       default:
         return 0.0;
diff --git a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index 8fb933a8af..5758c694a5 100644
--- a/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/wpilibj/wpilibJavaIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -167,7 +167,8 @@ public class MotorEncoderTest extends AbstractComsSetup {
 
 
   @Test
-  public void testPIDController() {
+  public void testPositionPIDController() {
+    me.getEncoder().setPIDSourceType(PIDSourceType.kDisplacement);
     PIDController pid = new PIDController(0.003, 0.001, 0, me.getEncoder(), me.getMotor());
     pid.setAbsoluteTolerance(50);
     pid.setOutputRange(-0.2, 0.2);
@@ -184,6 +185,26 @@ public class MotorEncoderTest extends AbstractComsSetup {
     pid.free();
   }
 
+  @Test
+  public void testVelocityPIDController() {
+    me.getEncoder().setPIDSourceType(PIDSourceType.kRate);
+    PIDController pid =
+        new PIDController(1e-5, 0.0, 3e-5, 8e-5, me.getEncoder(), me.getMotor());
+    pid.setAbsoluteTolerance(200);
+    pid.setToleranceBuffer(50);
+    pid.setOutputRange(-0.3, 0.3);
+    pid.setSetpoint(2000);
+
+    pid.enable();
+    Timer.delay(10.0);
+    pid.disable();
+
+    assertTrue(
+        "PID loop did not reach setpoint within 10 seconds. The error was: " + pid.getAvgError(),
+        pid.onTarget());
+
+    pid.free();
+  }
 
   /**
    * Checks to see if the encoders and counters are appropriately reset to zero
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index f0195223cd..f25f23b143 100644
--- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -31,6 +31,7 @@ public class AnalogInput extends SensorBase implements PIDSource,
 
 	private SimFloatInput m_impl;
 	private int m_channel;
+  protected PIDSourceParameter m_pidSource = PIDSourceParameter.kDistance;
 
 	/**
 	 * Construct an analog channel.
@@ -87,6 +88,20 @@ public class AnalogInput extends SensorBase implements PIDSource,
 		return m_channel;
 	}
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceParameter(PIDSourceParameter pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceParameter getPIDSourceParameter() {
+    return m_pidSource;
+  }
+
 	/**
 	 * Get the average value for use with PIDController
 	 *
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
index 6498ff5646..460ad8c2c4 100644
--- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -21,6 +21,7 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
     private double m_scale, m_offset;
     private AnalogInput m_analog_input;
     private boolean m_init_analog_input;
+  protected PIDSourceParameter m_pidSource = PIDSourceParameter.kDistance;
 
     /**
      * Common initialization code called by all constructors.
@@ -134,6 +135,20 @@ public class AnalogPotentiometer implements Potentiometer, LiveWindowSendable {
         return m_analog_input.getVoltage() * m_scale + m_offset;
     }
 
+  /**
+   * {@inheritDoc}
+   */
+  public void setPIDSourceParameter(PIDSourceParameter pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public PIDSourceParameter getPIDSourceParameter() {
+    return m_pidSource;
+  }
+
     /**
      * Implement the PIDSource interface.
      *
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Encoder.java
index d9ad52834d..4553ad1943 100644
--- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Encoder.java
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -280,6 +280,13 @@ public class Encoder extends SensorBase implements CounterBase, PIDSource, LiveW
 		m_pidSource = pidSource;
 	}
 
+    /**
+     * {@inheritDoc}
+     */
+    public PIDSourceParameter getPIDSourceParameter() {
+      return m_pidSource;
+    }
+
 	/**
 	 * Implement the PIDSource interface.
 	 *
diff --git a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Gyro.java b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Gyro.java
index 7deb144363..bfe13be102 100644
--- a/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Gyro.java
+++ b/wpilibj/wpilibJavaSim/src/main/java/edu/wpi/first/wpilibj/Gyro.java
@@ -118,6 +118,13 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
 		m_pidSource = pidSource;
 	}
 
+    /**
+     * {@inheritDoc}
+     */
+    public PIDSourceParameter getPIDSourceParameter() {
+      return m_pidSource;
+    }
+
 	/**
 	 * Get the angle of the gyro for use with PIDControllers
 	 *