artf4127: Implemented velocity PID controller

Change-Id: I8c0f84422f7ca0ac5c50fddb282fb3452ee1d491

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;

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;

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.
    *

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
    *

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.
    *

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();

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;

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;

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;
     }

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";

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;

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;