add stash shot

src/main/java/frc/robot/FieldConstants.java

@@ -12,6 +12,10 @@ public class FieldConstants {
   public static final double kFieldWidth = Constants.fieldLayout.getFieldWidth();
   public static final Pose2d trapPose =
       GeometryUtil.flipFieldPose(new Pose2d(11.896, 4.658, new Rotation2d(-2.093)));
+  public static final Translation2d stashPosition =
+      FieldConstants.Speaker.centerSpeakerOpening
+          .toTranslation2d()
+          .interpolate(new Translation2d(Units.inchesToMeters(72.455), kFieldWidth), 0.5);
 
   public static class NotePositions {
     private static final double kCenterlineX = kFieldLength / 2.0;

src/main/java/frc/robot/PoseEstimation.java

@@ -10,6 +10,7 @@
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.wpilibj.DriverStation;
+import frc.robot.arm.Arm.Joint;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.vision.Vision.VisionUpdate;
 import java.util.function.BooleanSupplier;
@@ -43,7 +44,7 @@ public record AimingParameters(
       Rotation2d armAngle,
       double effectiveDistance,
       double driveFeedVelocity,
-      int aimingJointIndex) {}
+      Joint aimingJointIndex) {}
 
   public PoseEstimation() {
     m_poseEstimator =
@@ -137,9 +138,11 @@ public AimingParameters getAimingParameters() {
         m_robotVelocity.dx * vehicleToGoalDirection.getSin() / targetDistance
             - m_robotVelocity.dy * vehicleToGoalDirection.getCos() / targetDistance;
 
-    int jointToAim = targetDistance < jointSwitchDistanceMeters ? 1 : 0;
+    Joint jointToAim = targetDistance < jointSwitchDistanceMeters ? Joint.kWrist : Joint.kElbow;
     double armAngle =
-        jointToAim == 1 ? wristAngleMap.get(targetDistance) : elbowAngleMap.get(targetDistance);
+        jointToAim == Joint.kWrist
+            ? wristAngleMap.get(targetDistance)
+            : elbowAngleMap.get(targetDistance);
 
     m_lastAimingParameters =
         new AimingParameters(

src/main/java/frc/robot/Robot.java

@@ -216,7 +216,12 @@ public Robot() {
         .onFalse(Commands.runOnce(() -> m_shooter.getCurrentCommand().cancel()));
     m_driverController
         .leftTrigger()
+        .and(m_drivetrain.inRangeOfGoal)
         .whileTrue(Superstructure.aimAtGoal(m_drivetrain, m_shooter, m_arm, m_lights));
+    m_driverController
+        .leftTrigger()
+        .and(m_drivetrain.inRangeOfGoal.negate())
+        .whileTrue(Superstructure.aimAtStash(m_drivetrain, m_shooter, m_arm, m_lights));
 
     m_driverController.start().onTrue(m_drivetrain.zeroGyro());
     m_operatorController

src/main/java/frc/robot/Superstructure.java

@@ -1,12 +1,15 @@
 package frc.robot;
 
+import com.pathplanner.lib.util.GeometryUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Command.InterruptionBehavior;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.ScheduleCommand;
 import frc.robot.arm.Arm;
 import frc.robot.arm.Arm.ArmSetpoints;
+import frc.robot.arm.Arm.Joint;
 import frc.robot.climber.Climber;
 import frc.robot.drivetrain.Drivetrain;
 import frc.robot.feeder.Feeder;
@@ -56,17 +59,28 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm,
             shooter.runShooter(),
             // arm.aim(
             //     () -> PoseEstimation.getInstance().getAimingParameters().aimingJointIndex(),
-            //     () ->
-            // PoseEstimation.getInstance().getAimingParameters().armAngle().getRadians()),
+            //     () -> PoseEstimation.getInstance().getAimingParameters().armAngle()),
             lights.showReadyToShootStatus(
-                drivetrain
-                    .atHeadingGoal
-                    .and(shooter.readyToShoot)
-                    .and(drivetrain.atHeadingGoal)
-                    .and(arm.atGoal)))
+                drivetrain.atHeadingGoal.and(shooter.readyToShoot).and(arm.atGoal)))
         .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
   }
 
+  public static Command aimAtStash(Drivetrain drivetrain, Shooter shooter, Arm arm, Lights lights) {
+    return Commands.parallel(
+        Commands.startEnd(
+            () ->
+                drivetrain.setHeadingGoal(
+                    () ->
+                        GeometryUtil.flipFieldPosition(FieldConstants.stashPosition)
+                            .minus(PoseEstimation.getInstance().getPose().getTranslation())
+                            .getAngle()),
+            drivetrain::clearHeadingGoal),
+        shooter.stashShot(),
+        arm.aim(() -> Joint.kElbow, () -> Rotation2d.fromDegrees(0.0)),
+        lights.showReadyToShootStatus(
+            drivetrain.atHeadingGoal.and(drivetrain.atHeadingGoal).and(arm.atGoal)));
+  }
+
   public static Command trapRoutine(Arm arm, Climber climber, Shooter shooter, Feeder feeder) {
     return Commands.sequence(
         new ScheduleCommand(arm.goToSetpoint(ArmSetpoints.kTrap)),

src/main/java/frc/robot/arm/Arm.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.filter.Debouncer.DebounceType;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -21,7 +22,6 @@
 import frc.lib.LoggedTunableNumber;
 import frc.robot.Constants;
 import java.util.function.DoubleSupplier;
-import java.util.function.IntSupplier;
 import java.util.function.Supplier;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -177,17 +177,23 @@ public Command storeInitialAngles() {
   }
 
   // Joint Index: 0 = elbow, 1 = wrist
-  public Command aim(IntSupplier jointIndexSupplier, DoubleSupplier angleSupplier) {
+  public Command aim(Supplier<Joint> jointIndexSupplier, Supplier<Rotation2d> angleSupplier) {
     return storeInitialAngles()
         .andThen(
             this.run(
                 () -> {
-                  if (jointIndexSupplier.getAsInt() == 0)
+                  if (jointIndexSupplier.get() == Joint.kElbow)
                     runSetpoint(
-                        angleSupplier.getAsDouble(), ArmSetpoints.kStowed.wristAngle, 0.0, 0.0);
-                  else if (jointIndexSupplier.getAsInt() == 1)
+                        angleSupplier.get().getRadians(),
+                        ArmSetpoints.kStowed.wristAngle,
+                        0.0,
+                        0.0);
+                  else if (jointIndexSupplier.get() == Joint.kWrist)
                     runSetpoint(
-                        ArmSetpoints.kStowed.elbowAngle, angleSupplier.getAsDouble(), 0.0, 0.0);
+                        ArmSetpoints.kStowed.elbowAngle,
+                        angleSupplier.get().getRadians(),
+                        0.0,
+                        0.0);
                   else throw new IllegalArgumentException("Invalid joint index.");
                 }))
         .withName("Aim (Arm)");
@@ -214,7 +220,7 @@ public Command aimElbowForTuning(DoubleSupplier driveEffort) {
         };
     return Commands.run(
             () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05)
-        .alongWith(aim(() -> 0, () -> state.currentPos))
+        .alongWith(aim(() -> Joint.kElbow, () -> Rotation2d.fromRadians(state.currentPos)))
         .finallyDo(() -> holdSetpoint().schedule());
   }
 
@@ -225,7 +231,7 @@ public Command aimWristForTuning(DoubleSupplier driveEffort) {
         };
     return Commands.run(
             () -> state.currentPos += MathUtil.applyDeadband(driveEffort.getAsDouble(), 0.1) * 0.05)
-        .alongWith(aim(() -> 1, () -> state.currentPos))
+        .alongWith(aim(() -> Joint.kWrist, () -> Rotation2d.fromRadians(state.currentPos)))
         .finallyDo(() -> holdSetpoint().schedule());
   }
 
@@ -293,7 +299,7 @@ public Command wristStaticCharacterization() {
         this, m_io::setWristCurrent, () -> Units.rotationsToRadians(m_inputs.wristVelocityRps));
   }
 
-  public static enum ArmSetpoints {
+  public enum ArmSetpoints {
     kStowed(-0.548, 2.485, 0.15, 0.0),
     kAmp(1.49 + 0.0873, Units.degreesToRadians(230), 0.0, 0.0),
     kClimb(1.633, -2.371, 0.0, 0.0),
@@ -312,4 +318,9 @@ private ArmSetpoints(
       this.wristDelay = wristDelay;
     }
   }
+
+  public enum Joint {
+    kElbow,
+    kWrist
+  }
 }

src/main/java/frc/robot/drivetrain/Drivetrain.java

@@ -58,7 +58,7 @@ public class Drivetrain extends SubsystemBase {
   public static final double kDriveBaseRadius =
       Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0);
   private static final LoggedTunableNumber inRangeRadius =
-      new LoggedTunableNumber("Drivetrain/InRangeRadius", 2.10);
+      new LoggedTunableNumber("Drivetrain/InRangeRadius", 5.0);
   private static final LoggedTunableNumber inRangeTolerance =
       new LoggedTunableNumber("Drivetrain/InRangeTolerance", 0.25);
   private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16);

src/main/java/frc/robot/shooter/Shooter.java

@@ -17,7 +17,7 @@
 
 public class Shooter extends SubsystemBase {
 
-  public static final double kShooterStepUp = Constants.kIsViper ? 0.0 : 0.0;
+  public static final double kShooterStepUp = Constants.kIsViper ? 1.0 : 1.0;
   public static final double kFarShotVelocityRpm = 5800.0;
   private final double kTrapShot = 400.0;
   private final double kAmpShot = 5000.0;
@@ -121,6 +121,15 @@ public Command trapShot() {
         .withName("Trap Shot (Shooter)");
   }
 
+  public Command stashShot() {
+    return this.run(
+            () -> {
+              m_io.setTopRollerVoltage(12.0);
+              m_io.setBottomRollerVoltage(12.0);
+            })
+        .withName("Stash Shot (Shooter)");
+  }
+
   public Command ampShot() {
     return this.run(() -> setRollersSetpointRpm(kAmpShot));
   }