From a6fc86e022a68a29cca531abf190ee9ed6d64bac Mon Sep 17 00:00:00 2001
From: Rongrui Zhu <ronrzhu@gmail.com>
Date: Sat, 14 Mar 2026 17:46:46 -0700
Subject: [PATCH] Still give other motors some velocity

---
 .../subsystems/shooter/ShooterSubsystem.java  | 22 ++++++++++++++++---
 1 file changed, 19 insertions(+), 3 deletions(-)

diff --git a/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java
index 0054fa4f..94ee325c 100644
--- a/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java
+++ b/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java
@@ -8,6 +8,7 @@
 import xbot.common.command.SimpleWaitForMaintainerCommand;
 import xbot.common.controls.actuators.XCANMotorController;
 import xbot.common.controls.actuators.XCANMotorControllerPIDProperties;
+import xbot.common.properties.AngularVelocityProperty;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
 import xbot.common.resiliency.DeviceHealth;
@@ -19,8 +20,6 @@
 import java.util.List;
 
 import static edu.wpi.first.units.Units.RPM;
-import static edu.wpi.first.units.Units.Rotations;
-import static edu.wpi.first.units.Units.RotationsPerSecond;
 import static edu.wpi.first.units.Units.Seconds;
 
 @Singleton
@@ -39,6 +38,8 @@ public class ShooterSubsystem extends BaseSetpointSubsystem<AngularVelocity, Dou
 
     public AngularVelocity currentTargetVelocity = RPM.of(0);
 
+    public AngularVelocityProperty lowPowerModeVelocity;
+
     private final Subsystem trimSetpointLock = new Subsystem() {
 
     };
@@ -120,6 +121,7 @@ public ShooterSubsystem(XCANMotorController.XCANMotorControllerFactory xcanMotor
         this.defaultShootingVelocity = this.propertyFactory.createPersistentProperty("Default Shooter Velocity RPM", 3000);
         this.trimValue = this.propertyFactory.createPersistentProperty("Shooter Trim Value", 0);
         this.readinessTimeoutSeconds = this.propertyFactory.createPersistentProperty("Readiness Timeout Seconds", 2.0);
+        this.lowPowerModeVelocity = this.propertyFactory.createPersistentProperty("LowPowerModeVelocity", RPM.of(150));
     }
 
     public void stop() {
@@ -141,9 +143,23 @@ public void decreaseShooterOffset() {
     }
 
     public void runMotorsAtVelocity(AngularVelocity velocity) {
-        for (var motor : getHealthyShooterMotors()) {
+        var allMotors = getShooterMotors();
+        var healthyMotors = getHealthyShooterMotors();
+        for (var motor : healthyMotors) {
             motor.setVelocityTarget(velocity);
         }
+
+        if (isInLowPowerMode) {
+            // In low power mode, we want all the other motors that are NOT our
+            // main healthy motor, that is still healthy.
+            var otherMotors = allMotors.stream()
+                    .filter(motor -> !healthyMotors.contains(motor)
+                            && motor.getHealth() == DeviceHealth.Healthy)
+                    .toList();
+            for (var motor : otherMotors) {
+                motor.setVelocityTarget(lowPowerModeVelocity.get());
+            }
+        }
     }
 
     public boolean isReadyToFire() {