[oldCommands] Add wrappers for WPILib objects to work with old PID Controller (#3710)

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogAccelerometer.java

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.AnalogAccelerometer;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/** Wrapper so that PIDSource is implemented for AnalogAccelerometer for old PIDController. */
+public class PIDAnalogAccelerometer extends AnalogAccelerometer implements PIDSource {
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDAnalogAccelerometer(int channel) {
+    super(channel);
+  }
+
+  public PIDAnalogAccelerometer(AnalogInput channel) {
+    super(channel);
+  }
+
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
+  /**
+   * Get the Acceleration for the PID Source parent.
+   *
+   * @return The current acceleration in Gs.
+   */
+  @Override
+  public double pidGet() {
+    return getAcceleration();
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogGyro.java

@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/** Wrapper so that PIDSource is implemented for AnalogGyro for old PIDController. */
+public class PIDAnalogGyro extends AnalogGyro implements PIDSource {
+  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDAnalogGyro(int channel) {
+    super(channel);
+  }
+
+  public PIDAnalogGyro(AnalogInput channel) {
+    super(channel);
+  }
+
+  public PIDAnalogGyro(int channel, int center, double offset) {
+    super(channel, center, offset);
+  }
+
+  public PIDAnalogGyro(AnalogInput channel, int center, double offset) {
+    super(channel, center, offset);
+  }
+
+  /**
+   * Set which parameter of the gyro you are using as a process control variable. The Gyro class
+   * supports the rate and displacement parameters
+   *
+   * @param pidSource An enum to select the parameter.
+   */
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  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 PIDSourceType
+   *
+   * @return the output according to the gyro
+   */
+  @Override
+  public double pidGet() {
+    switch (m_pidSource) {
+      case kRate:
+        return getRate();
+      case kDisplacement:
+        return getAngle();
+      default:
+        return 0.0;
+    }
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogInput.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/** Wrapper so that PIDSource is implemented for AnalogInput for old PIDController. */
+public class PIDAnalogInput extends AnalogInput implements PIDSource {
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDAnalogInput(int channel) {
+    super(channel);
+  }
+
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
+  /**
+   * Get the average voltage for use with PIDController.
+   *
+   * @return the average voltage
+   */
+  @Override
+  public double pidGet() {
+    return getAverageVoltage();
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDAnalogPotentiometer.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/** Wrapper so that PIDSource is implemented for AnalogPotentiometer for old PIDController. */
+public class PIDAnalogPotentiometer extends AnalogPotentiometer implements PIDSource {
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDAnalogPotentiometer(int channel, double fullRange, double offset) {
+    super(channel, fullRange, offset);
+  }
+
+  public PIDAnalogPotentiometer(AnalogInput input, double fullRange, double offset) {
+    super(input, fullRange, offset);
+  }
+
+  public PIDAnalogPotentiometer(int channel, double scale) {
+    super(channel, scale);
+  }
+
+  public PIDAnalogPotentiometer(AnalogInput input, double scale) {
+    super(input, scale);
+  }
+
+  public PIDAnalogPotentiometer(int channel) {
+    super(channel);
+  }
+
+  public PIDAnalogPotentiometer(AnalogInput input) {
+    super(input);
+  }
+
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+      throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
+    }
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
+  /**
+   * Implement the PIDSource interface.
+   *
+   * @return The current reading.
+   */
+  @Override
+  public double pidGet() {
+    return get();
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDEncoder.java

@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.DigitalSource;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+
+/** Wrapper so that PIDSource is implemented for Encoder for old PIDController. */
+public class PIDEncoder extends Encoder implements PIDSource {
+  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDEncoder(final int channelA, final int channelB, boolean reverseDirection) {
+    super(channelA, channelB, reverseDirection, EncodingType.k4X);
+  }
+
+  public PIDEncoder(final int channelA, final int channelB) {
+    super(channelA, channelB, false);
+  }
+
+  public PIDEncoder(
+      final int channelA,
+      final int channelB,
+      boolean reverseDirection,
+      final EncodingType encodingType) {
+    super(channelA, channelB, reverseDirection, encodingType);
+  }
+
+  public PIDEncoder(
+      final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) {
+    super(channelA, channelB, indexChannel, reverseDirection);
+  }
+
+  public PIDEncoder(final int channelA, final int channelB, final int indexChannel) {
+    super(channelA, channelB, indexChannel, false);
+  }
+
+  public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) {
+    super(sourceA, sourceB, reverseDirection, EncodingType.k4X);
+  }
+
+  public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB) {
+    super(sourceA, sourceB, false);
+  }
+
+  public PIDEncoder(
+      DigitalSource sourceA,
+      DigitalSource sourceB,
+      boolean reverseDirection,
+      final EncodingType encodingType) {
+    super(sourceA, sourceB, reverseDirection, encodingType);
+  }
+
+  public PIDEncoder(
+      DigitalSource sourceA,
+      DigitalSource sourceB,
+      DigitalSource indexSource,
+      boolean reverseDirection) {
+    super(sourceA, sourceB, indexSource, reverseDirection);
+  }
+
+  public PIDEncoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) {
+    super(sourceA, sourceB, indexSource, false);
+  }
+
+  /**
+   * Set which parameter of the encoder you are using as a process control variable. The encoder
+   * class supports the rate and distance parameters.
+   *
+   * @param pidSource An enum to select the parameter.
+   */
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
+  /**
+   * Implement the PIDSource interface.
+   *
+   * @return The current value of the selected source parameter.
+   */
+  @Override
+  public double pidGet() {
+    switch (m_pidSource) {
+      case kDisplacement:
+        return getDistance();
+      case kRate:
+        return getRate();
+      default:
+        return 0.0;
+    }
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDMotorController.java

@@ -0,0 +1,108 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.wpilibj.PIDOutput;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+
+/** Wrapper so that PIDOutput is implemented for MotorController for old PIDController. */
+public class PIDMotorController implements PIDOutput, MotorController, Sendable {
+  private final MotorController m_motorController;
+
+  public PIDMotorController(MotorController motorController) {
+    m_motorController = motorController;
+  }
+
+  /**
+   * Write out the PID value as seen in the PIDOutput base object.
+   *
+   * @param output Write out the PWM value as was found in the PIDController
+   */
+  @Override
+  public void pidWrite(double output) {
+    m_motorController.set(output);
+  }
+
+  /**
+   * Common interface for setting the speed of a motor controller.
+   *
+   * @param speed The speed to set. Value should be between -1.0 and 1.0.
+   */
+  @Override
+  public void set(double speed) {
+    m_motorController.set(speed);
+  }
+
+  /**
+   * Sets the voltage output of the MotorController. Compensates for the current bus voltage to
+   * ensure that the desired voltage is output even if the battery voltage is below 12V - highly
+   * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
+   * calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+   * properly - unlike the ordinary set function, it is not "set it and forget it."
+   *
+   * @param outputVolts The voltage to output.
+   */
+  @Override
+  public void setVoltage(double outputVolts) {
+    m_motorController.setVoltage(outputVolts);
+  }
+
+  /**
+   * Common interface for getting the current set speed of a motor controller.
+   *
+   * @return The current set speed. Value is between -1.0 and 1.0.
+   */
+  @Override
+  public double get() {
+    return m_motorController.get();
+  }
+
+  /**
+   * Common interface for inverting direction of a motor controller.
+   *
+   * @param isInverted The state of inversion true is inverted.
+   */
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_motorController.setInverted(isInverted);
+  }
+
+  /**
+   * Common interface for returning if a motor controller is in the inverted state or not.
+   *
+   * @return isInverted The state of the inversion true is inverted.
+   */
+  @Override
+  public boolean getInverted() {
+    return m_motorController.getInverted();
+  }
+
+  /** Disable the motor controller. */
+  @Override
+  public void disable() {
+    m_motorController.disable();
+  }
+
+  /**
+   * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+   * motor.
+   */
+  @Override
+  public void stopMotor() {
+    m_motorController.stopMotor();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Motor Controller");
+    builder.setActuator(true);
+    builder.setSafeState(this::disable);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}

wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/pidwrappers/PIDUltrasonic.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.pidwrappers;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.Ultrasonic;
+
+/** Wrapper so that PIDSource is implemented for Ultrasonic for old PIDController. */
+public class PIDUltrasonic extends Ultrasonic implements PIDSource {
+  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
+
+  public PIDUltrasonic(int pingChannel, int echoChannel) {
+    super(pingChannel, echoChannel);
+  }
+
+  public PIDUltrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
+    super(pingChannel, echoChannel);
+  }
+
+  @Override
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
+      throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
+    }
+    m_pidSource = pidSource;
+  }
+
+  @Override
+  public PIDSourceType getPIDSourceType() {
+    return m_pidSource;
+  }
+
+  /**
+   * Get the range in the inches for the PIDSource base object.
+   *
+   * @return The range in inches
+   */
+  @Override
+  public double pidGet() {
+    return getRangeInches();
+  }
+}