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

wpilibc/src/main/native/include/frc/drive/MecanumDrive.h

@@ -4,10 +4,12 @@
 
 #pragma once
 
+#include <functional>
 #include <memory>
 #include <string>
 
 #include <units/angle.h>
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -69,15 +71,39 @@ class MecanumDrive : public RobotDriveBase,
     double rearRight = 0.0;
   };
 
+  WPI_IGNORE_DEPRECATED
+
   /**
    * Construct a MecanumDrive.
    *
    * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor.
+   * @param rearLeftMotor Rear-left motor.
+   * @param frontRightMotor Front-right motor.
+   * @param rearRightMotor Rear-right motor.
    */
   MecanumDrive(MotorController& frontLeftMotor, MotorController& rearLeftMotor,
                MotorController& frontRightMotor,
                MotorController& rearRightMotor);
 
+  WPI_UNIGNORE_DEPRECATED
+
+  /**
+   * Construct a MecanumDrive.
+   *
+   * If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor Front-left motor setter.
+   * @param rearLeftMotor Rear-left motor setter.
+   * @param frontRightMotor Front-right motor setter.
+   * @param rearRightMotor Rear-right motor setter.
+   */
+  MecanumDrive(std::function<void(double)> frontLeftMotor,
+               std::function<void(double)> rearLeftMotor,
+               std::function<void(double)> frontRightMotor,
+               std::function<void(double)> rearRightMotor);
+
   ~MecanumDrive() override = default;
 
   MecanumDrive(MecanumDrive&&) = default;
@@ -141,10 +167,16 @@ class MecanumDrive : public RobotDriveBase,
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  MotorController* m_frontLeftMotor;
-  MotorController* m_rearLeftMotor;
-  MotorController* m_frontRightMotor;
-  MotorController* m_rearRightMotor;
+  std::function<void(double)> m_frontLeftMotor;
+  std::function<void(double)> m_rearLeftMotor;
+  std::function<void(double)> m_frontRightMotor;
+  std::function<void(double)> m_rearRightMotor;
+
+  // Used for Sendable property getters
+  double m_frontLeftOutput = 0.0;
+  double m_rearLeftOutput = 0.0;
+  double m_frontRightOutput = 0.0;
+  double m_rearRightOutput = 0.0;
 
   bool reported = false;
 };