[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/cpp/drive/DifferentialDrive.cpp

@@ -16,11 +16,21 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 DifferentialDrive::DifferentialDrive(MotorController& leftMotor,
                                      MotorController& rightMotor)
-    : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
-  wpi::SendableRegistry::AddChild(this, m_leftMotor);
-  wpi::SendableRegistry::AddChild(this, m_rightMotor);
+    : DifferentialDrive{[&](double output) { leftMotor.Set(output); },
+                        [&](double output) { rightMotor.Set(output); }} {
+  wpi::SendableRegistry::AddChild(this, &leftMotor);
+  wpi::SendableRegistry::AddChild(this, &rightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
+                                     std::function<void(double)> rightMotor)
+    : m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
@@ -40,8 +50,11 @@ void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
 
   auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-  m_leftMotor->Set(left * m_maxOutput);
-  m_rightMotor->Set(right * m_maxOutput);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -60,8 +73,11 @@ void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
 
   auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-  m_leftMotor->Set(left * m_maxOutput);
-  m_rightMotor->Set(right * m_maxOutput);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -80,8 +96,11 @@ void DifferentialDrive::TankDrive(double leftSpeed, double rightSpeed,
 
   auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
 
-  m_leftMotor->Set(left * m_maxOutput);
-  m_rightMotor->Set(right * m_maxOutput);
+  m_leftOutput = left * m_maxOutput;
+  m_rightOutput = right * m_maxOutput;
+
+  m_leftMotor(m_leftOutput);
+  m_rightMotor(m_rightOutput);
 
   Feed();
 }
@@ -157,8 +176,12 @@ DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK(
 }
 
 void DifferentialDrive::StopMotor() {
-  m_leftMotor->StopMotor();
-  m_rightMotor->StopMotor();
+  m_leftOutput = 0.0;
+  m_rightOutput = 0.0;
+
+  m_leftMotor(0.0);
+  m_rightMotor(0.0);
+
   Feed();
 }
 
@@ -171,9 +194,7 @@ void DifferentialDrive::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetActuator(true);
   builder.SetSafeState([=, this] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Left Motor Speed", [=, this] { return m_leftMotor->Get(); },
-      [=, this](double value) { m_leftMotor->Set(value); });
+      "Left Motor Speed", [&] { return m_leftOutput; }, m_leftMotor);
   builder.AddDoubleProperty(
-      "Right Motor Speed", [=, this] { return m_rightMotor->Get(); },
-      [=, this](double value) { m_rightMotor->Set(value); });
+      "Right Motor Speed", [&] { return m_rightOutput; }, m_rightMotor);
 }

wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp

@@ -16,18 +16,32 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 MecanumDrive::MecanumDrive(MotorController& frontLeftMotor,
                            MotorController& rearLeftMotor,
                            MotorController& frontRightMotor,
                            MotorController& rearRightMotor)
-    : m_frontLeftMotor(&frontLeftMotor),
-      m_rearLeftMotor(&rearLeftMotor),
-      m_frontRightMotor(&frontRightMotor),
-      m_rearRightMotor(&rearRightMotor) {
-  wpi::SendableRegistry::AddChild(this, m_frontLeftMotor);
-  wpi::SendableRegistry::AddChild(this, m_rearLeftMotor);
-  wpi::SendableRegistry::AddChild(this, m_frontRightMotor);
-  wpi::SendableRegistry::AddChild(this, m_rearRightMotor);
+    : MecanumDrive{[&](double output) { frontLeftMotor.Set(output); },
+                   [&](double output) { rearLeftMotor.Set(output); },
+                   [&](double output) { frontRightMotor.Set(output); },
+                   [&](double output) { rearRightMotor.Set(output); }} {
+  wpi::SendableRegistry::AddChild(this, &frontLeftMotor);
+  wpi::SendableRegistry::AddChild(this, &rearLeftMotor);
+  wpi::SendableRegistry::AddChild(this, &frontRightMotor);
+  wpi::SendableRegistry::AddChild(this, &rearRightMotor);
+}
+
+WPI_UNIGNORE_DEPRECATED
+
+MecanumDrive::MecanumDrive(std::function<void(double)> frontLeftMotor,
+                           std::function<void(double)> rearLeftMotor,
+                           std::function<void(double)> frontRightMotor,
+                           std::function<void(double)> rearRightMotor)
+    : m_frontLeftMotor{std::move(frontLeftMotor)},
+      m_rearLeftMotor{std::move(rearLeftMotor)},
+      m_frontRightMotor{std::move(frontRightMotor)},
+      m_rearRightMotor{std::move(rearRightMotor)} {
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
@@ -47,10 +61,15 @@ void MecanumDrive::DriveCartesian(double xSpeed, double ySpeed,
   auto [frontLeft, frontRight, rearLeft, rearRight] =
       DriveCartesianIK(xSpeed, ySpeed, zRotation, gyroAngle);
 
-  m_frontLeftMotor->Set(frontLeft * m_maxOutput);
-  m_frontRightMotor->Set(frontRight * m_maxOutput);
-  m_rearLeftMotor->Set(rearLeft * m_maxOutput);
-  m_rearRightMotor->Set(rearRight * m_maxOutput);
+  m_frontLeftOutput = frontLeft * m_maxOutput;
+  m_rearLeftOutput = rearLeft * m_maxOutput;
+  m_frontRightOutput = frontRight * m_maxOutput;
+  m_rearRightOutput = rearRight * m_maxOutput;
+
+  m_frontLeftMotor(m_frontLeftOutput);
+  m_frontRightMotor(m_frontRightOutput);
+  m_rearLeftMotor(m_rearLeftOutput);
+  m_rearRightMotor(m_rearRightOutput);
 
   Feed();
 }
@@ -68,10 +87,16 @@ void MecanumDrive::DrivePolar(double magnitude, Rotation2d angle,
 }
 
 void MecanumDrive::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(0.0);
+  m_frontRightMotor(0.0);
+  m_rearLeftMotor(0.0);
+  m_rearRightMotor(0.0);
+
   Feed();
 }
 
@@ -108,15 +133,15 @@ void MecanumDrive::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetActuator(true);
   builder.SetSafeState([=, this] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Front Left Motor Speed", [=, this] { return m_frontLeftMotor->Get(); },
-      [=, this](double value) { m_frontLeftMotor->Set(value); });
+      "Front Left Motor Speed", [&] { return m_frontLeftOutput; },
+      m_frontLeftMotor);
   builder.AddDoubleProperty(
-      "Front Right Motor Speed", [=, this] { return m_frontRightMotor->Get(); },
-      [=, this](double value) { m_frontRightMotor->Set(value); });
+      "Front Right Motor Speed", [&] { return m_frontRightOutput; },
+      m_frontRightMotor);
   builder.AddDoubleProperty(
-      "Rear Left Motor Speed", [=, this] { return m_rearLeftMotor->Get(); },
-      [=, this](double value) { m_rearLeftMotor->Set(value); });
+      "Rear Left Motor Speed", [&] { return m_rearLeftOutput; },
+      m_rearLeftMotor);
   builder.AddDoubleProperty(
-      "Rear Right Motor Speed", [=, this] { return m_rearRightMotor->Get(); },
-      [=, this](double value) { m_rearRightMotor->Set(value); });
+      "Rear Right Motor Speed", [&] { return m_rearRightOutput; },
+      m_rearRightMotor);
 }

wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp

@@ -12,6 +12,8 @@ using namespace frc;
 // Can't use a delegated constructor here because of an MSVC bug.
 // https://developercommunity.visualstudio.com/content/problem/583/compiler-bug-with-delegating-a-constructor.html
 
+WPI_IGNORE_DEPRECATED
+
 MotorControllerGroup::MotorControllerGroup(
     std::vector<std::reference_wrapper<MotorController>>&& motorControllers)
     : m_motorControllers(std::move(motorControllers)) {
@@ -74,3 +76,5 @@ void MotorControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
       "Value", [=, this] { return Get(); },
       [=, this](double value) { Set(value); });
 }
+
+WPI_UNIGNORE_DEPRECATED

wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp

@@ -11,6 +11,8 @@
 
 using namespace frc;
 
+WPI_IGNORE_DEPRECATED
+
 NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
     : m_dio(dioChannel), m_pwm(pwmChannel) {
   wpi::SendableRegistry::AddChild(this, &m_dio);
@@ -26,6 +28,8 @@ NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
   wpi::SendableRegistry::AddLW(this, "Nidec Brushless", pwmChannel);
 }
 
+WPI_UNIGNORE_DEPRECATED
+
 void NidecBrushless::Set(double speed) {
   if (!m_disabled) {
     m_speed = speed;

wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp

@@ -8,13 +8,31 @@
 #include <wpi/sendable/SendableBuilder.h>
 #include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/RobotController.h"
+
 using namespace frc;
 
 void PWMMotorController::Set(double speed) {
-  m_pwm.SetSpeed(m_isInverted ? -speed : speed);
+  if (m_isInverted) {
+    speed = -speed;
+  }
+  m_pwm.SetSpeed(speed);
+
+  for (auto& follower : m_nonowningFollowers) {
+    follower->Set(speed);
+  }
+  for (auto& follower : m_owningFollowers) {
+    follower->Set(speed);
+  }
+
   Feed();
 }
 
+void PWMMotorController::SetVoltage(units::volt_t output) {
+  // NOLINTNEXTLINE(bugprone-integer-division)
+  Set(output / RobotController::GetBatteryVoltage());
+}
+
 double PWMMotorController::Get() const {
   return m_pwm.GetSpeed() * (m_isInverted ? -1.0 : 1.0);
 }
@@ -48,11 +66,19 @@ void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
   m_pwm.EnableDeadbandElimination(eliminateDeadband);
 }
 
+void PWMMotorController::AddFollower(PWMMotorController& follower) {
+  m_nonowningFollowers.emplace_back(&follower);
+}
+
+WPI_IGNORE_DEPRECATED
+
 PWMMotorController::PWMMotorController(std::string_view name, int channel)
     : m_pwm(channel, false) {
   wpi::SendableRegistry::AddLW(this, name, channel);
 }
 
+WPI_UNIGNORE_DEPRECATED
+
 void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Motor Controller");
   builder.SetActuator(true);