Results after Shooter Testing

src/main/java/competition/commandgroups/PrepareToFireAtAmpCommandGroup.java

@@ -14,7 +14,7 @@ public class PrepareToFireAtAmpCommandGroup extends ParallelCommandGroup {
     public PrepareToFireAtAmpCommandGroup(SetArmAngleCommand armAngle,
                                           WarmUpShooterCommand shooter) {
         // Move arm to preset position
-        armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.FIRING_IN_AMP);
+        armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.FIRING_FROM_AMP);
         this.addCommands(armAngle);
         // Set shooter wheels to target RPM
         shooter.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT);

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

@@ -9,6 +9,7 @@
 import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.arm.commands.ArmMaintainerCommand;
 import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand;
+import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand;
 import competition.subsystems.arm.commands.SetArmAngleCommand;
 import competition.subsystems.arm.commands.SetArmExtensionCommand;
 import competition.subsystems.collector.commands.EjectCollectorCommand;
@@ -23,6 +24,8 @@
 import competition.subsystems.schoocher.commands.IntakeScoocherCommand;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
 import competition.subsystems.shooter.ShooterWheelTargetSpeeds;
+import competition.subsystems.shooter.commands.ContinuouslyWarmUpForSpeakerCommand;
+import competition.subsystems.shooter.commands.FireWhenReadyCommand;
 import competition.subsystems.shooter.commands.WarmUpShooterCommand;
 import competition.subsystems.shooter.commands.WarmUpShooterRPMCommand;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -53,41 +56,44 @@ public void setupFundamentalCommands(
             EjectScoocherCommand scoocherEject,
             IntakeCollectorCommand collectorIntake,
             EjectCollectorCommand collectorEject,
-            WarmUpShooterCommand shooterWarmUp,
+            WarmUpShooterCommand shooterWarmUpSafe,
+            WarmUpShooterCommand shooterWarmUpNear,
+            WarmUpShooterCommand shooterWarmUpFar,
+            WarmUpShooterCommand shooterWarmUpAmp,
             FireCollectorCommand fireCollectorCommand,
             SetArmAngleCommand armAngle,
             ArmMaintainerCommand armMaintainer,
-            WarmUpShooterCommand warmUpShooterSlow,
-            WarmUpShooterCommand warmUpShooterFast,
             WarmUpShooterRPMCommand warmUpShooterDifferentialRPM
     ) {
         // Scooch
-        oi.operatorGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get());
-        oi.operatorGamepad.getXboxButton(XboxButton.LeftBumper).whileTrue(scoocherEject);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(scoocherIntakeProvider.get());
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.LeftBumper).whileTrue(scoocherEject);
 
         // Collect
-        oi.operatorGamepad.getXboxButton(XboxButton.RightTrigger).whileTrue(collectorIntake);
-        oi.operatorGamepad.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectorEject);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.RightTrigger).whileTrue(collectorIntake);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectorEject);
 
         // Fire
-        shooterWarmUp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT);
-        oi.operatorGamepad.getXboxButton(XboxButton.X).whileTrue(shooterWarmUp);
-        oi.operatorGamepad.getXboxButton(XboxButton.A).whileTrue(fireCollectorCommand);
+        shooterWarmUpSafe.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER);
+        shooterWarmUpNear.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT);
+        shooterWarmUpFar.setTargetRpm(ShooterWheelSubsystem.TargetRPM.DISTANCESHOT);
+        shooterWarmUpAmp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT);
+
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.A).whileTrue(shooterWarmUpSafe);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.X).whileTrue(shooterWarmUpNear);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Y).whileTrue(shooterWarmUpFar);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Back).whileTrue(shooterWarmUpAmp);
+
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.B).whileTrue(fireCollectorCommand);
 
         // Arms are taken care of via their maintainer & human overrides.
         armAngle.setArmPosition(ArmSubsystem.UsefulArmPosition.SCOOCH_NOTE);
         var scoochNote = scoocherIntakeProvider.get();
         scoochNote.alongWith(armAngle);
         // TODO: bind scoochNote action to a button in operatorGamepad
 
-        warmUpShooterSlow.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SAFE);
-        warmUpShooterFast.setTargetRpm(ShooterWheelSubsystem.TargetRPM.NEARSHOT);
-
         warmUpShooterDifferentialRPM.setTargetRpm(new ShooterWheelTargetSpeeds(1000, 2000));
-
-        oi.operatorGamepad.getPovIfAvailable(270).whileTrue(warmUpShooterSlow);
-        oi.operatorGamepad.getPovIfAvailable(90).whileTrue(warmUpShooterFast);
-        oi.operatorGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM);
+        //oi.operatorFundamentalsGamepad.getPovIfAvailable(0).whileTrue(warmUpShooterDifferentialRPM);
     }
 
     // Example for setting up a command to fire when a button is pressed:
@@ -199,21 +205,90 @@ public void setupArmPIDCommands(
         var highArm = commandProvider.get();
         highArm.setTargetExtension(150);
 
-        var increaseArm = commandProvider.get();
-        increaseArm.setTargetExtension(20);
-        increaseArm.setRelative(true);
+        var increaseArmLarge = commandProvider.get();
+        increaseArmLarge.setTargetExtension(20);
+        increaseArmLarge.setRelative(true);
+
+        var increaseArmSmall = commandProvider.get();
+        increaseArmSmall.setTargetExtension(2);
+        increaseArmSmall.setRelative(true);
+
+        var decreaseArmLarge = commandProvider.get();
+        decreaseArmLarge.setTargetExtension(-20);
+        decreaseArmLarge.setRelative(true);
+
+        var decreaseArmSmall = commandProvider.get();
+        decreaseArmSmall.setTargetExtension(-2);
+        decreaseArmSmall.setRelative(true);
+
+        oi.operatorFundamentalsGamepad.getPovIfAvailable(0).whileTrue(increaseArmLarge);
+        oi.operatorFundamentalsGamepad.getPovIfAvailable(180).whileTrue(decreaseArmLarge);
+        oi.operatorFundamentalsGamepad.getPovIfAvailable(90).whileTrue(increaseArmSmall);
+        oi.operatorFundamentalsGamepad.getPovIfAvailable(270).whileTrue(decreaseArmSmall);
+        oi.operatorFundamentalsGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand);
+    }
+
+    @Inject
+    public void setupAdvancedOperatorCommands(
+            OperatorInterface oi,
+            ArmSubsystem arm,
+            Provider<SetArmExtensionCommand> setArmExtensionCommandProvider,
+            IntakeCollectorCommand intakeCollector,
+            EjectCollectorCommand ejectCollector,
+            IntakeScoocherCommand intakeScoocher,
+            EjectScoocherCommand ejectScoocher,
+            Provider<WarmUpShooterCommand> warmUpShooterCommandProvider,
+            ContinuouslyPointArmAtSpeakerCommand continuouslyPointArmAtSpeaker,
+            ContinuouslyWarmUpForSpeakerCommand continuouslyWarmUpForSpeaker,
+            FireWhenReadyCommand fireWhenReady
+    ) {
+        //Useful arm positions
+        var armToCollection = setArmExtensionCommandProvider.get();
+        armToCollection.setTargetExtension(arm.getUsefulArmPositionExtensionInMm(
+                ArmSubsystem.UsefulArmPosition.COLLECTING_FROM_GROUND));
+
+        var armToScooch = setArmExtensionCommandProvider.get();
+        armToScooch.setTargetExtension(arm.getUsefulArmPositionExtensionInMm(
+                ArmSubsystem.UsefulArmPosition.SCOOCH_NOTE));
+
+        var armToSubwoofer = setArmExtensionCommandProvider.get();
+        armToSubwoofer.setTargetExtension(arm.getUsefulArmPositionExtensionInMm(
+                ArmSubsystem.UsefulArmPosition.FIRING_FROM_SUBWOOFER));
+
+        var armToAmp = setArmExtensionCommandProvider.get();
+        armToAmp.setTargetExtension(arm.getUsefulArmPositionExtensionInMm(
+                ArmSubsystem.UsefulArmPosition.FIRING_FROM_AMP));
+
+        // Useful wheel speeds
+        var warmUpShooterSubwoofer = warmUpShooterCommandProvider.get();
+        warmUpShooterSubwoofer.setTargetRpm(ShooterWheelSubsystem.TargetRPM.SUBWOOFER);
+
+        var warmUpShooterAmp = warmUpShooterCommandProvider.get();
+        warmUpShooterAmp.setTargetRpm(ShooterWheelSubsystem.TargetRPM.AMP_SHOT);
+
+        // Combine into useful actions
+        // Note manipulation:
+        var collectNote = intakeCollector.alongWith(armToCollection);
+        var scoochNote = intakeScoocher.alongWith(armToScooch);
+
+        // Preparing to score:
+        var prepareToFireAtSubwoofer = warmUpShooterSubwoofer.alongWith(armToSubwoofer);
+        var prepareToFireAtAmp = warmUpShooterAmp.alongWith(armToAmp);
+        var continuouslyPrepareToFireAtSpeaker =
+                continuouslyWarmUpForSpeaker.alongWith(continuouslyPointArmAtSpeaker);
 
-        var decreaseArm = commandProvider.get();
-        decreaseArm.setTargetExtension(-20);
-        decreaseArm.setRelative(true);
+        // Bind to buttons
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.LeftTrigger).whileTrue(collectNote);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Back).whileTrue(ejectCollector);
 
-        oi.operatorGamepadSecond.getXboxButton(XboxButton.A).onTrue(decreaseArm);
-        oi.operatorGamepadSecond.getXboxButton(XboxButton.Y).onTrue(increaseArm);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.LeftBumper).whileTrue(scoochNote);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Start).whileTrue(ejectScoocher);
 
-        oi.operatorGamepadSecond.getXboxButton(XboxButton.X).onTrue(homeArm);
-        oi.operatorGamepadSecond.getXboxButton(XboxButton.B).onTrue(highArm);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.X).whileTrue(prepareToFireAtSubwoofer);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.Y).whileTrue(prepareToFireAtAmp);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.A).whileTrue(continuouslyPrepareToFireAtSpeaker);
 
-        oi.operatorGamepad.getXboxButton(XboxButton.Start).onTrue(calibrateArmsManuallyCommand);
+        oi.operatorGamepadAdvanced.getXboxButton(XboxButton.RightTrigger).whileTrue(fireWhenReady);
     }
 
     private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand(

src/main/java/competition/operator_interface/OperatorInterface.java

@@ -20,8 +20,8 @@ public class OperatorInterface {
 
     // ONE GAMEPAD IS FOR COMPETITION, SECOND GAMEPAD IS USED DURING PRACTICE
     public XXboxController driverGamepad;
-    public XXboxController operatorGamepad;
-    public XXboxController operatorGamepadSecond;
+    public XXboxController operatorFundamentalsGamepad;
+    public XXboxController operatorGamepadAdvanced;
 
     public XXboxController autoGamepad;
     public XJoystick neoTrellis;
@@ -41,13 +41,13 @@ public OperatorInterface(XXboxControllerFactory controllerFactory,
         driverGamepad.setLeftInversion(false, true);
         driverGamepad.setRightInversion(true, true);
 
-        operatorGamepad = controllerFactory.create(1);
-        operatorGamepad.setLeftInversion(false, true);
-        operatorGamepad.setRightInversion(false, true);
+        operatorFundamentalsGamepad = controllerFactory.create(1);
+        operatorFundamentalsGamepad.setLeftInversion(false, true);
+        operatorFundamentalsGamepad.setRightInversion(false, true);
 
-        operatorGamepadSecond = controllerFactory.create(4);
-        operatorGamepadSecond.setLeftInversion(false, true);
-        operatorGamepadSecond.setRightInversion(false, true);
+        operatorGamepadAdvanced = controllerFactory.create(4);
+        operatorGamepadAdvanced.setLeftInversion(false, true);
+        operatorGamepadAdvanced.setRightInversion(false, true);
 
         autoGamepad = controllerFactory.create(3);
 

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -16,6 +16,7 @@
 import xbot.common.controls.actuators.XSolenoid;
 import xbot.common.controls.sensors.XSparkAbsoluteEncoder;
 import xbot.common.controls.sensors.XTimer;
+import xbot.common.math.DoubleInterpolator;
 import xbot.common.math.MathUtils;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
@@ -88,11 +89,13 @@ public enum LimitState {
     public enum UsefulArmPosition {
         STARTING_POSITION,
         COLLECTING_FROM_GROUND,
-        FIRING_FROM_SPEAKER_FRONT,
-        FIRING_IN_AMP,
+        FIRING_FROM_SUBWOOFER,
+        FIRING_FROM_AMP,
         SCOOCH_NOTE
     }
 
+    private DoubleInterpolator speakerDistanceToExtensionInterpolator;
+
     @Inject
     public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory,
                         XSolenoid.XSolenoidFactory xSolenoidFactory,
@@ -166,8 +169,8 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
             armMotorRight.setForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, true);
             armMotorRight.setReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen, false);
 
-            armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kBrake);
-            armMotorRight.setIdleMode(CANSparkBase.IdleMode.kBrake);
+            armMotorLeft.setIdleMode(CANSparkBase.IdleMode.kCoast);
+            armMotorRight.setIdleMode(CANSparkBase.IdleMode.kCoast);
         }
 
         this.armState = ArmState.STOPPED;
@@ -180,6 +183,10 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa
             .append(new MechanismLigament2d("box-top", 2, 90))
             .append(new MechanismLigament2d("box-left", 1, 90));
 
+        speakerDistanceToExtensionInterpolator =
+                new DoubleInterpolator(
+                        new double[]{0, 36, 49.5, 63, 80, 111, 136},
+                        new double[]{0, 0,  20.0, 26, 41, 57,  64});
     }
 
     public double constrainPowerIfNearLimit(double power, double actualPosition) {
@@ -374,14 +381,34 @@ public double getUsefulArmPositionAngle(UsefulArmPosition usefulArmPosition) {
             // THESE ARE ALL PLACEHOLDER VALUES!!!
             case STARTING_POSITION -> angle = 40;
             case COLLECTING_FROM_GROUND -> angle = 0;
-            case FIRING_FROM_SPEAKER_FRONT -> angle = 30;
-            case FIRING_IN_AMP -> angle = 80;
+            case FIRING_FROM_SUBWOOFER -> angle = 30;
+            case FIRING_FROM_AMP -> angle = 80;
             case SCOOCH_NOTE -> angle = 60; // placeholder value, safe angle to let note through while still low
             default -> angle = 40;
         }
         return angle;
     }
 
+    public double getUsefulArmPositionExtensionInMm(UsefulArmPosition usefulArmPosition) {
+        double extension = 0;
+        switch (usefulArmPosition) {
+            case STARTING_POSITION:
+            case COLLECTING_FROM_GROUND:
+            case FIRING_FROM_SUBWOOFER:
+                extension = 0;
+                break;
+            case FIRING_FROM_AMP:
+                extension = upperLegalLimitMm.get();
+                break;
+            case SCOOCH_NOTE:
+                extension = 15;
+                break;
+            default:
+                return 0;
+        }
+        return extension;
+    }
+
 
     public LimitState getLimitState(XCANSparkMax motor) {
         boolean upperHit = false;
@@ -510,6 +537,21 @@ public void setRampingPowerEnabled(boolean enabled) {
         powerRampingEnabled = enabled;
     }
 
+    public double getAngleFromRange() {
+        return getArmAngleFromDistance(pose.getDistanceFromSpeaker());
+    }
+
+    public void markArmsAsCalibratedAgainstLowerPhyscalLimit() {
+        hasCalibratedLeft = true;
+        armMotorLeftRevolutionOffset = -armMotorLeft.getPosition();
+        hasCalibratedRight = true;
+        armMotorRightRevolutionOffset = -armMotorRight.getPosition();
+    }
+
+    public double getRecommendedExtension(double distanceFromSpeaker) {
+        return speakerDistanceToExtensionInterpolator.getInterpolatedOutputVariable(distanceFromSpeaker);
+    }
+
     public void periodic() {
         if (contract.isArmReady()) {
             recordArmEncoderValues();
@@ -542,15 +584,4 @@ public void refreshDataFrame() {
             armAbsoluteEncoder.refreshDataFrame();
         }
     }
-
-    public double getAngleFromRange() {
-        return getArmAngleFromDistance(pose.getDistanceFromSpeaker());
-    }
-
-    public void markArmsAsCalibratedAgainstLowerPhyscalLimit() {
-        hasCalibratedLeft = true;
-        armMotorLeftRevolutionOffset = -armMotorLeft.getPosition();
-        hasCalibratedRight = true;
-        armMotorRightRevolutionOffset = -armMotorRight.getPosition();
-    }
 }

src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java

@@ -30,7 +30,7 @@ public ArmMaintainerCommand(ArmSubsystem arm, PropertyFactory pf,
                                 HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory,
                                 PIDManager.PIDManagerFactory pidf,
                                 CalibrationDecider.CalibrationDeciderFactory calf, OperatorInterface oi){
-        super(arm, pf, hvmFactory, 1, .001);
+        super(arm, pf, hvmFactory, 1, 0.5);
         this.arm = arm;
         this.oi = oi;
         pf.setPrefix(this);
@@ -120,7 +120,7 @@ protected double getErrorMagnitude() {
     @Override
     protected Double getHumanInput() {
         return MathUtils.deadband(
-                oi.operatorGamepad.getLeftVector().y,
+                oi.operatorFundamentalsGamepad.getLeftVector().y,
                 oi.getOperatorGamepadTypicalDeadband(),
                 (x) -> x);
     }

src/main/java/competition/subsystems/arm/commands/ContinuouslyPointArmAtSpeakerCommand.java

@@ -0,0 +1,31 @@
+package competition.subsystems.arm.commands;
+
+import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.pose.PoseSubsystem;
+import xbot.common.command.BaseCommand;
+import xbot.common.command.BaseSetpointCommand;
+
+import javax.inject.Inject;
+
+public class ContinuouslyPointArmAtSpeakerCommand extends BaseSetpointCommand {
+
+    ArmSubsystem arm;
+    PoseSubsystem pose;
+
+    @Inject
+    public ContinuouslyPointArmAtSpeakerCommand(ArmSubsystem arm, PoseSubsystem pose) {
+        super(arm);
+        this.arm = arm;
+        this.pose = pose;
+    }
+
+    @Override
+    public void initialize() {
+        log.info("Initializing");
+    }
+
+    @Override
+    public void execute() {
+        arm.setTargetValue(arm.getRecommendedExtension(pose.getDistanceFromSpeaker()));
+    }
+}