[wpilib] Add functional interface equivalents to MotorController (#6053)

This does not deprecate any current functionality, but prepares the way for future deprecation.

The drive classes now accept void(double) functions, which makes them more flexible.

The C++ API ended up a bit more verbose, but the Java API is really concise with method references, which is >80% of our userbase. For example:

`DifferentialDrive drive = new DifferentialDrive(m_leftMotor::set, m_rightMotor::set);`

Lambdas can be passed to interoperate with vendor motor controller APIs that don't have e.g., set(double), so CTRE doesn't have to maintain their WPI_ classes anymore.

MotorControllerGroup was replaced with PWMMotorController.addFollower() for PWM motor controllers. Users of CAN motor controllers should use their vendor's follower functionality.

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java

@@ -14,49 +14,16 @@ import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
 
 /**
  * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
  * base, "tank drive", or West Coast Drive.
  *
  * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
- * (e.g., 6WD or 8WD). This class takes a MotorController per side. For four and six motor
- * drivetrains, construct and pass in {@link
- * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup} instances as follows.
- *
- * <p>Four motor drivetrain:
- *
- * <pre><code>
- * public class Robot {
- *   MotorController m_frontLeft = new PWMVictorSPX(1);
- *   MotorController m_rearLeft = new PWMVictorSPX(2);
- *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_rearLeft);
- *
- *   MotorController m_frontRight = new PWMVictorSPX(3);
- *   MotorController m_rearRight = new PWMVictorSPX(4);
- *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_rearRight);
- *
- *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
- * }
- * </code></pre>
- *
- * <p>Six motor drivetrain:
- *
- * <pre><code>
- * public class Robot {
- *   MotorController m_frontLeft = new PWMVictorSPX(1);
- *   MotorController m_midLeft = new PWMVictorSPX(2);
- *   MotorController m_rearLeft = new PWMVictorSPX(3);
- *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
- *
- *   MotorController m_frontRight = new PWMVictorSPX(4);
- *   MotorController m_midRight = new PWMVictorSPX(5);
- *   MotorController m_rearRight = new PWMVictorSPX(6);
- *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_midRight, m_rearRight);
- *
- *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
- * }
- * </code></pre>
+ * (e.g., 6WD or 8WD). This class takes a setter per side. For four and six motor drivetrains, use
+ * CAN motor controller followers or {@link
+ * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}.
  *
  * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
  *
@@ -88,8 +55,12 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   private static int instances;
 
-  private final MotorController m_leftMotor;
-  private final MotorController m_rightMotor;
+  private final DoubleConsumer m_leftMotor;
+  private final DoubleConsumer m_rightMotor;
+
+  // Used for Sendable property getters
+  private double m_leftOutput;
+  private double m_rightOutput;
 
   private boolean m_reported;
 
@@ -128,15 +99,30 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
    * @param leftMotor Left motor.
    * @param rightMotor Right motor.
    */
-  @SuppressWarnings("this-escape")
+  @SuppressWarnings({"removal", "this-escape"})
   public DifferentialDrive(MotorController leftMotor, MotorController rightMotor) {
+    this((double output) -> leftMotor.set(output), (double output) -> rightMotor.set(output));
+    SendableRegistry.addChild(this, leftMotor);
+    SendableRegistry.addChild(this, rightMotor);
+  }
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * <p>To pass multiple motors per side, use CAN motor controller followers or {@link
+   * edu.wpi.first.wpilibj.motorcontrol.PWMMotorController#addFollower(PWMMotorController)}. If a
+   * motor needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
+   */
+  @SuppressWarnings("this-escape")
+  public DifferentialDrive(DoubleConsumer leftMotor, DoubleConsumer rightMotor) {
     requireNonNullParam(leftMotor, "leftMotor", "DifferentialDrive");
     requireNonNullParam(rightMotor, "rightMotor", "DifferentialDrive");
 
     m_leftMotor = leftMotor;
     m_rightMotor = rightMotor;
-    SendableRegistry.addChild(this, m_leftMotor);
-    SendableRegistry.addChild(this, m_rightMotor);
     instances++;
     SendableRegistry.addLW(this, "DifferentialDrive", instances);
   }
@@ -178,8 +164,11 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
 
     var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -207,8 +196,11 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
 
     var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -245,8 +237,11 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
 
     var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
 
-    m_leftMotor.set(speeds.left * m_maxOutput);
-    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_leftOutput = speeds.left * m_maxOutput;
+    m_rightOutput = speeds.right * m_maxOutput;
+
+    m_leftMotor.accept(m_leftOutput);
+    m_rightMotor.accept(m_rightOutput);
 
     feed();
   }
@@ -351,8 +346,12 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
 
   @Override
   public void stopMotor() {
-    m_leftMotor.stopMotor();
-    m_rightMotor.stopMotor();
+    m_leftOutput = 0.0;
+    m_rightOutput = 0.0;
+
+    m_leftMotor.accept(0.0);
+    m_rightMotor.accept(0.0);
+
     feed();
   }
 
@@ -366,7 +365,7 @@ public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoC
     builder.setSmartDashboardType("DifferentialDrive");
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
-    builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
-    builder.addDoubleProperty("Right Motor Speed", m_rightMotor::get, m_rightMotor::set);
+    builder.addDoubleProperty("Left Motor Speed", () -> m_leftOutput, m_leftMotor::accept);
+    builder.addDoubleProperty("Right Motor Speed", () -> m_rightOutput, m_rightMotor::accept);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java

@@ -16,6 +16,7 @@ import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import java.util.function.DoubleConsumer;
 
 /**
  * A class for driving Mecanum drive platforms.
@@ -56,10 +57,16 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   private static int instances;
 
-  private final MotorController m_frontLeftMotor;
-  private final MotorController m_rearLeftMotor;
-  private final MotorController m_frontRightMotor;
-  private final MotorController m_rearRightMotor;
+  private final DoubleConsumer m_frontLeftMotor;
+  private final DoubleConsumer m_rearLeftMotor;
+  private final DoubleConsumer m_frontRightMotor;
+  private final DoubleConsumer m_rearRightMotor;
+
+  // Used for Sendable property getters
+  private double m_frontLeftOutput;
+  private double m_rearLeftOutput;
+  private double m_frontRightOutput;
+  private double m_rearRightOutput;
 
   private boolean m_reported;
 
@@ -104,12 +111,39 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
    * @param frontRightMotor The motor on the front-right corner.
    * @param rearRightMotor The motor on the rear-right corner.
    */
-  @SuppressWarnings("this-escape")
+  @SuppressWarnings({"removal", "this-escape"})
   public MecanumDrive(
       MotorController frontLeftMotor,
       MotorController rearLeftMotor,
       MotorController frontRightMotor,
       MotorController rearRightMotor) {
+    this(
+        (double output) -> frontLeftMotor.set(output),
+        (double output) -> rearLeftMotor.set(output),
+        (double output) -> frontRightMotor.set(output),
+        (double output) -> rearRightMotor.set(output));
+    SendableRegistry.addChild(this, frontLeftMotor);
+    SendableRegistry.addChild(this, rearLeftMotor);
+    SendableRegistry.addChild(this, frontRightMotor);
+    SendableRegistry.addChild(this, rearRightMotor);
+  }
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * <p>If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor The setter for the motor on the front-left corner.
+   * @param rearLeftMotor The setter for the motor on the rear-left corner.
+   * @param frontRightMotor The setter for the motor on the front-right corner.
+   * @param rearRightMotor The setter for the motor on the rear-right corner.
+   */
+  @SuppressWarnings("this-escape")
+  public MecanumDrive(
+      DoubleConsumer frontLeftMotor,
+      DoubleConsumer rearLeftMotor,
+      DoubleConsumer frontRightMotor,
+      DoubleConsumer rearRightMotor) {
     requireNonNullParam(frontLeftMotor, "frontLeftMotor", "MecanumDrive");
     requireNonNullParam(rearLeftMotor, "rearLeftMotor", "MecanumDrive");
     requireNonNullParam(frontRightMotor, "frontRightMotor", "MecanumDrive");
@@ -119,10 +153,6 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
     m_rearLeftMotor = rearLeftMotor;
     m_frontRightMotor = frontRightMotor;
     m_rearRightMotor = rearRightMotor;
-    SendableRegistry.addChild(this, m_frontLeftMotor);
-    SendableRegistry.addChild(this, m_rearLeftMotor);
-    SendableRegistry.addChild(this, m_frontRightMotor);
-    SendableRegistry.addChild(this, m_rearRightMotor);
     instances++;
     SendableRegistry.addLW(this, "MecanumDrive", instances);
   }
@@ -172,10 +202,15 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
 
     var speeds = driveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
 
-    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
-    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
-    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
-    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
+    m_frontLeftOutput = speeds.frontLeft * m_maxOutput;
+    m_rearLeftOutput = speeds.rearLeft * m_maxOutput;
+    m_frontRightOutput = speeds.frontRight * m_maxOutput;
+    m_rearRightOutput = speeds.rearRight * m_maxOutput;
+
+    m_frontLeftMotor.accept(m_frontLeftOutput);
+    m_frontRightMotor.accept(m_frontRightOutput);
+    m_rearLeftMotor.accept(m_rearLeftOutput);
+    m_rearRightMotor.accept(m_rearRightOutput);
 
     feed();
   }
@@ -256,10 +291,16 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
 
   @Override
   public void stopMotor() {
-    m_frontLeftMotor.stopMotor();
-    m_frontRightMotor.stopMotor();
-    m_rearLeftMotor.stopMotor();
-    m_rearRightMotor.stopMotor();
+    m_frontLeftOutput = 0.0;
+    m_frontRightOutput = 0.0;
+    m_rearLeftOutput = 0.0;
+    m_rearRightOutput = 0.0;
+
+    m_frontLeftMotor.accept(0.0);
+    m_frontRightMotor.accept(0.0);
+    m_rearLeftMotor.accept(0.0);
+    m_rearRightMotor.accept(0.0);
+
     feed();
   }
 
@@ -274,11 +315,12 @@ public class MecanumDrive extends RobotDriveBase implements Sendable, AutoClosea
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
     builder.addDoubleProperty(
-        "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
+        "Front Left Motor Speed", () -> m_frontLeftOutput, m_frontLeftMotor::accept);
     builder.addDoubleProperty(
-        "Front Right Motor Speed", m_frontRightMotor::get, m_frontRightMotor::set);
-    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
+        "Front Right Motor Speed", () -> m_frontRightOutput, m_frontRightMotor::accept);
     builder.addDoubleProperty(
-        "Rear Right Motor Speed", m_rearRightMotor::get, m_rearRightMotor::set);
+        "Rear Left Motor Speed", () -> m_rearLeftOutput, m_rearLeftMotor::accept);
+    builder.addDoubleProperty(
+        "Rear Right Motor Speed", () -> m_rearRightOutput, m_rearRightMotor::accept);
   }
 }

wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java

@@ -9,7 +9,14 @@ import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Arrays;
 
-/** Allows multiple {@link MotorController} objects to be linked together. */
+/**
+ * Allows multiple {@link MotorController} objects to be linked together.
+ *
+ * @deprecated Use CAN motor controller followers or {@link
+ *     PWMMotorController#addFollower(PWMMotorController)}.
+ */
+@SuppressWarnings("removal")
+@Deprecated(forRemoval = true, since = "2024")
 public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
   private final MotorController[] m_motorControllers;

wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java

@@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj.MotorSafety;
 import edu.wpi.first.wpilibj.PWM;
 
 /** Nidec Brushless Motor. */
+@SuppressWarnings("removal")
 public class NidecBrushless extends MotorSafety
     implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;

wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java

@@ -9,11 +9,14 @@ import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.MotorSafety;
 import edu.wpi.first.wpilibj.PWM;
+import java.util.ArrayList;
 
 /** Common base class for all PWM Motor Controllers. */
+@SuppressWarnings("removal")
 public abstract class PWMMotorController extends MotorSafety
     implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
+  private final ArrayList<PWMMotorController> m_followers = new ArrayList<>();
   protected PWM m_pwm;
 
   /**
@@ -46,7 +49,15 @@ public abstract class PWMMotorController extends MotorSafety
    */
   @Override
   public void set(double speed) {
-    m_pwm.setSpeed(m_isInverted ? -speed : speed);
+    if (m_isInverted) {
+      speed = -speed;
+    }
+    m_pwm.setSpeed(speed);
+
+    for (var follower : m_followers) {
+      follower.set(speed);
+    }
+
     feed();
   }
 
@@ -117,6 +128,15 @@ public abstract class PWMMotorController extends MotorSafety
     m_pwm.enableDeadbandElimination(eliminateDeadband);
   }
 
+  /**
+   * Make the given PWM motor controller follow the output of this one.
+   *
+   * @param follower The motor controller follower.
+   */
+  public void addFollower(PWMMotorController follower) {
+    m_followers.add(follower);
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Motor Controller");