Feature/shooter dynamic error ranges

src/main/java/competition/injection/components/BaseRobotComponent.java

@@ -10,6 +10,7 @@
 import competition.subsystems.collector.CollectorSubsystem;
 import competition.subsystems.schoocher.ScoocherSubsystem;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
+import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand;
 import xbot.common.injection.swerve.SwerveComponentHolder;
 import competition.operator_interface.OperatorCommandMap;
 import competition.operator_interface.OperatorInterface;
@@ -51,4 +52,5 @@ public abstract class BaseRobotComponent extends BaseComponent {
     public abstract ShooterWheelSubsystem shooterSubsystem();
 
     public abstract Simulator2024 simulator2024();
+    public abstract ShooterWheelMaintainerCommand shooterWheelMaintainerCommand();
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -315,7 +315,7 @@ public void setupAdvancedOperatorCommands(
         oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Y).whileTrue(prepareToFireAtAmp);
         oi.operatorGamepadAdvanced.getXboxButton(XboxButton.A).whileTrue(continuouslyPrepareToFireAtSpeaker);
 
-        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightTrigger).whileTrue(fireWhenReady);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightTrigger).whileTrue(fireWhenReady.repeatedly());
     }
 
     @Inject

src/main/java/competition/subsystems/shooter/ShooterWheelSubsystem.java

@@ -159,6 +159,7 @@ public ShooterWheelTargetSpeeds getTargetValue() {
     @Override
     public void setTargetValue(ShooterWheelTargetSpeeds value) {
         targetRpms = value;
+        log.info("Target RPMs: " + value.upperWheelsTargetRPM + ", " + value.lowerWheelsTargetRPM);
     }
 
     public void setTargetValue(double value) {

src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java

@@ -1,10 +1,12 @@
 package competition.subsystems.shooter.commands;
 
 import competition.operator_interface.OperatorInterface;
+import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
 import competition.subsystems.shooter.ShooterWheelTargetSpeeds;
 import xbot.common.command.BaseMaintainerCommand;
 import xbot.common.logic.HumanVsMachineDecider;
+import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
 
 import javax.inject.Inject;
@@ -13,13 +15,21 @@ public class ShooterWheelMaintainerCommand extends BaseMaintainerCommand<Shooter
 
     final ShooterWheelSubsystem wheel;
     final OperatorInterface oi;
+    final ArmSubsystem arm;
+
+    final DoubleProperty subwooferRpmErrorTolerance;
+    final DoubleProperty ampRpmErrorTolerance;
 
     @Inject
     public ShooterWheelMaintainerCommand(OperatorInterface oi, ShooterWheelSubsystem wheel, PropertyFactory pf,
-                                         HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory) {
+                                         HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, ArmSubsystem arm) {
         super(wheel, pf, hvmFactory, 150, 0.5);
         this.oi = oi;
         this.wheel = wheel;
+        this.arm = arm;
+
+        subwooferRpmErrorTolerance = pf.createPersistentProperty("Subwoofer RPM Error Tolerance", 1000);
+        ampRpmErrorTolerance = pf.createPersistentProperty("Amp RPM Error Tolerance", 1000);
     }
 
     @Override
@@ -77,4 +87,21 @@ protected double getErrorMagnitude() {
         return ((targets.upperWheelsTargetRPM - currents.upperWheelsTargetRPM)
                 + (targets.lowerWheelsTargetRPM - currents.lowerWheelsTargetRPM)) / 2;
     }
+
+    @Override
+    public boolean getErrorWithinTolerance() {
+        // If the arm is up really high, we are scoring in the amp and can have a big error tolerance.
+
+        if (arm.getExtensionDistance() > 125) {
+            return Math.abs(getErrorMagnitude()) < ampRpmErrorTolerance.get();
+        }
+
+        // If the arm is at its minimum, we are scoring from Subwoofer and can have a large-ish error tolerance.
+        if (arm.getExtensionDistance() < 10) {
+            return Math.abs(getErrorMagnitude()) < subwooferRpmErrorTolerance.get();
+        }
+
+        // Other shots likely require more precision.
+        return super.getErrorWithinTolerance();
+    }
 }

src/test/java/competition/subsystems/shooter/ShooterWheelMaintainerCommandTest.java

@@ -0,0 +1,64 @@
+package competition.subsystems.shooter;
+
+import competition.BaseCompetitionTest;
+import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand;
+import org.junit.Test;
+import xbot.common.controls.actuators.mock_adapters.MockCANSparkMax;
+
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+public class ShooterWheelMaintainerCommandTest extends BaseCompetitionTest {
+
+    ShooterWheelMaintainerCommand command;
+    ShooterWheelSubsystem wheel;
+    ArmSubsystem arm;
+
+    @Override
+    public void setUp() {
+        super.setUp();
+        command = this.getInjectorComponent().shooterWheelMaintainerCommand();
+        wheel = this.getInjectorComponent().shooterSubsystem();
+        arm = this.getInjectorComponent().armSubsystem();
+    }
+
+    @Test
+    public void testDifferentThresholds() {
+        command.initialize();
+        command.execute();
+
+        wheel.setTargetValue(3000);
+        setWheelSpeed(2500);
+        moveArm(15);
+        wheel.refreshDataFrame();
+        arm.refreshDataFrame();
+
+        assertFalse(command.getErrorWithinTolerance());
+
+        setWheelSpeed(2500);
+        moveArm(0);
+        wheel.refreshDataFrame();
+        arm.refreshDataFrame();
+
+        assertTrue(command.getErrorWithinTolerance());
+
+        setWheelSpeed(2500);
+        moveArm(1000);
+        wheel.refreshDataFrame();
+        arm.refreshDataFrame();
+
+        assertTrue(command.getErrorWithinTolerance());
+    }
+
+
+    private void setWheelSpeed(double speed) {
+        ((MockCANSparkMax)wheel.upperWheelMotor).setVelocity(speed);
+        ((MockCANSparkMax)wheel.lowerWheelMotor).setVelocity(speed);
+    }
+
+    private void moveArm(double position) {
+        ((MockCANSparkMax)arm.armMotorLeft).setPosition(position);
+        ((MockCANSparkMax)arm.armMotorRight).setPosition(position);
+    }
+}