[wpilib] Add functional interface equivalents to MotorController

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

@@ -16,11 +16,20 @@
 
 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_UNIGNORE_DEPRECATED
+
+DifferentialDrive::DifferentialDrive(std::function<void(double)> leftMotor,
+                                     std::function<void(double)> rightMotor)
+    : m_leftMotor{std::move(leftMotor)}, m_rightMotor{std::move(rightMotor)} {
+  // wpi::SendableRegistry::AddChild(this, m_leftMotor);
+  // wpi::SendableRegistry::AddChild(this, m_rightMotor);
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
@@ -40,8 +49,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 +72,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 +95,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 +175,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 +193,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,31 @@
 
 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_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)} {
+  // wpi::SendableRegistry::AddChild(this, m_frontLeftMotor);
+  // wpi::SendableRegistry::AddChild(this, m_rearLeftMotor);
+  // wpi::SendableRegistry::AddChild(this, m_frontRightMotor);
+  // wpi::SendableRegistry::AddChild(this, m_rearRightMotor);
   static int instances = 0;
   ++instances;
   wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
@@ -47,10 +60,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 +86,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 +132,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,28 @@
 #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_followers) {
+    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 +63,19 @@ void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
   m_pwm.EnableDeadbandElimination(eliminateDeadband);
 }
 
+void PWMMotorController::AddFollower(PWMMotorController& follower) {
+  m_followers.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);

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

@@ -4,8 +4,10 @@
 
 #pragma once
 
+#include <functional>
 #include <string>
 
+#include <wpi/deprecated.h>
 #include <wpi/sendable/Sendable.h>
 #include <wpi/sendable/SendableHelper.h>
 
@@ -20,43 +22,9 @@ class MotorController;
  * the Kit of Parts drive base, "tank drive", or West Coast Drive.
  *
  * 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
- * MotorControllerGroup instances as follows.
- *
- * Four motor drivetrain:
- * @code{.cpp}
- * class Robot {
- *  public:
- *   frc::PWMVictorSPX m_frontLeft{1};
- *   frc::PWMVictorSPX m_rearLeft{2};
- *   frc::MotorControllerGroup m_left{m_frontLeft, m_rearLeft};
- *
- *   frc::PWMVictorSPX m_frontRight{3};
- *   frc::PWMVictorSPX m_rearRight{4};
- *   frc::MotorControllerGroup m_right{m_frontRight, m_rearRight};
- *
- *   frc::DifferentialDrive m_drive{m_left, m_right};
- * };
- * @endcode
- *
- * Six motor drivetrain:
- * @code{.cpp}
- * class Robot {
- *  public:
- *   frc::PWMVictorSPX m_frontLeft{1};
- *   frc::PWMVictorSPX m_midLeft{2};
- *   frc::PWMVictorSPX m_rearLeft{3};
- *   frc::MotorControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
- *
- *   frc::PWMVictorSPX m_frontRight{4};
- *   frc::PWMVictorSPX m_midRight{5};
- *   frc::PWMVictorSPX m_rearRight{6};
- *   frc::MotorControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
- *
- *   frc::DifferentialDrive m_drive{m_left, m_right};
- * };
- * @endcode
+ * wheels per side (e.g., 6WD or 8WD). This class takes a setter per side. For
+ * four and six motor drivetrains, use CAN motor controller followers or
+ * PWMMotorController::AddFollower().
  *
  * A differential drive robot has left and right wheels separated by an
  * arbitrary width.
@@ -101,13 +69,34 @@ class DifferentialDrive : public RobotDriveBase,
     double right = 0.0;
   };
 
+  WPI_IGNORE_DEPRECATED
+
+  /**
+   * Construct a DifferentialDrive.
+   *
+   * To pass multiple motors per side, use CAN motor controller followers or
+   * PWMSpeedController::AddFollower(). If a motor needs to be inverted, do so
+   * before passing it in.
+   *
+   * @param leftMotor Left motor.
+   * @param rightMotor Right motor.
+   */
+  WPI_DEPRECATED("Use DifferentialDrive constructor with function arguments.")
+  DifferentialDrive(MotorController& leftMotor, MotorController& rightMotor);
+
+  WPI_UNIGNORE_DEPRECATED
+
   /**
    * Construct a DifferentialDrive.
    *
    * To pass multiple motors per side, use a MotorControllerGroup. If a motor
    * needs to be inverted, do so before passing it in.
+   *
+   * @param leftMotor Left motor setter.
+   * @param rightMotor Right motor setter.
    */
-  DifferentialDrive(MotorController& leftMotor, MotorController& rightMotor);
+  DifferentialDrive(std::function<void(double)> leftMotor,
+                    std::function<void(double)> rightMotor);
 
   ~DifferentialDrive() override = default;
 
@@ -210,8 +199,12 @@ class DifferentialDrive : public RobotDriveBase,
   void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  MotorController* m_leftMotor;
-  MotorController* m_rightMotor;
+  std::function<void(double)> m_leftMotor;
+  std::function<void(double)> m_rightMotor;
+
+  // Used for Sendable property getters
+  double m_leftOutput = 0.0;
+  double m_rightOutput = 0.0;
 };
 
 }  // namespace frc