From 65c8fbd45216b18458b1f44a43dd86431cd97ba2 Mon Sep 17 00:00:00 2001
From: Peter Johnson <johnson.peter@gmail.com>
Date: Tue, 6 Sep 2022 08:18:33 -0700
Subject: [PATCH] [wpilib] MotorControllerGroup: Override setVoltage (#4403)

This causes setVoltage to be called on the lower level motor contollers,
which is benefical in cases when they are smart motor controllers.
Previously, the default implementation (using the bus voltage and
calling set()) was used in this case.

This does slightly pessimize the case when the lower level motor
controllers use the default setVoltage implementation, but given the
prevalence of smart motor controllers, this seems like an overall win.
---
 .../main/native/cpp/motorcontrol/MotorControllerGroup.cpp  | 6 ++++++
 .../native/include/frc/motorcontrol/MotorControllerGroup.h | 1 +
 .../first/wpilibj/motorcontrol/MotorControllerGroup.java   | 7 +++++++
 3 files changed, 14 insertions(+)

diff --git a/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp b/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
index b7f26d18f6..32a8a2f0d0 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
@@ -33,6 +33,12 @@ void MotorControllerGroup::Set(double speed) {
   }
 }
 
+void MotorControllerGroup::SetVoltage(units::volt_t output) {
+  for (auto motorController : m_motorControllers) {
+    motorController.get().SetVoltage(m_isInverted ? -output : output);
+  }
+}
+
 double MotorControllerGroup::Get() const {
   if (!m_motorControllers.empty()) {
     return m_motorControllers.front().get().Get() * (m_isInverted ? -1 : 1);
diff --git a/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h b/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
index 6f47bf938a..99a9e4e164 100644
--- a/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
+++ b/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
@@ -31,6 +31,7 @@ class MotorControllerGroup : public wpi::Sendable,
   MotorControllerGroup& operator=(MotorControllerGroup&&) = default;
 
   void Set(double speed) override;
+  void SetVoltage(units::volt_t output) override;
   double Get() const override;
   void SetInverted(bool isInverted) override;
   bool GetInverted() const override;
diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
index 714aac5c85..d9da3c8f1f 100644
--- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
+++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
@@ -54,6 +54,13 @@ public class MotorControllerGroup implements MotorController, Sendable, AutoClos
     }
   }
 
+  @Override
+  public void setVoltage(double outputVolts) {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.setVoltage(m_isInverted ? -outputVolts : outputVolts);
+    }
+  }
+
   @Override
   public double get() {
     if (m_motorControllers.length > 0) {