fix stash shot position

team4909 · Apr 1, 2024 · 2ae06d1 · 2ae06d1
1 parent 4e2236a
commit 2ae06d1
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 9 deletions.
diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java
@@ -6,16 +6,22 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.util.Units;
+import java.util.function.Supplier;
 
 public class FieldConstants {
   public static final double kFieldLength = Constants.fieldLayout.getFieldLength();
   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 =
+  private static final Translation2d stashPosition =
       FieldConstants.Speaker.centerSpeakerOpening
           .toTranslation2d()
           .interpolate(new Translation2d(Units.inchesToMeters(72.455), kFieldWidth), 0.5);
+  public static final Supplier<Translation2d> stashPositionSupplier =
+      () ->
+          Constants.onRedAllianceSupplier.getAsBoolean()
+              ? GeometryUtil.flipFieldPosition(stashPosition)
+              : stashPosition;
 
   public static class NotePositions {
     private static final double kCenterlineX = kFieldLength / 2.0;

diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java
@@ -1,6 +1,5 @@
 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;
@@ -20,7 +19,6 @@
 /** Class of factories for commands that combine multiple subsystems. */
 public class Superstructure {
   public static Command sensorIntake(Feeder feeder, Intake intake) {
-    // In sim just wait 5 seconds since we don't have a sensor
     final Command feedUntilHasNote =
         Commands.either(
             feeder.feed().until(intake.hasIntookPieceSim),
@@ -57,9 +55,9 @@ public static Command aimAtGoal(Drivetrain drivetrain, Shooter shooter, Arm arm,
                         () -> PoseEstimation.getInstance().getAimingParameters().driveHeading()),
                 drivetrain::clearHeadingGoal),
             shooter.runShooter(),
-            // arm.aim(
-            //     () -> PoseEstimation.getInstance().getAimingParameters().aimingJointIndex(),
-            //     () -> PoseEstimation.getInstance().getAimingParameters().armAngle()),
+            arm.aim(
+                () -> PoseEstimation.getInstance().getAimingParameters().aimingJointIndex(),
+                () -> PoseEstimation.getInstance().getAimingParameters().armAngle()),
             lights.showReadyToShootStatus(
                 drivetrain.atHeadingGoal.and(shooter.readyToShoot).and(arm.atGoal)))
         .withInterruptBehavior(InterruptionBehavior.kCancelIncoming);
@@ -71,7 +69,8 @@ public static Command aimAtStash(Drivetrain drivetrain, Shooter shooter, Arm arm
             () ->
                 drivetrain.setHeadingGoal(
                     () ->
-                        GeometryUtil.flipFieldPosition(FieldConstants.stashPosition)
+                        FieldConstants.stashPositionSupplier
+                            .get()
                             .minus(PoseEstimation.getInstance().getPose().getTranslation())
                             .getAngle()),
             drivetrain::clearHeadingGoal),

diff --git a/src/main/java/frc/robot/arm/Arm.java b/src/main/java/frc/robot/arm/Arm.java
@@ -31,7 +31,7 @@ public class Arm extends SubsystemBase {
   public static final double kCatchWristAngleRad = 2.264 - Units.degreesToRadians(5.0);
   public static final double kSubwooferWristAngleRad = 2.083;
   public static final double kDeployGizmoAngleRad = Units.degreesToRadians(80.0);
-  private final double kJointTolerenceDegrees = 1.0;
+  private final double kJointTolerenceDegrees = 2.0;
 
   private final ArmIO m_io;
   private final ArmIOInputsAutoLogged m_inputs = new ArmIOInputsAutoLogged();
@@ -262,7 +262,11 @@ public Command goToSetpoint(ArmSetpoints setpoint) {
   }
 
   public Command idle() {
-    return this.run(() -> m_io.stop()).withName("Idle (Arm)");
+    return this.run(m_io::stop).withName("Idle (Arm)");
+  }
+
+  public Command stop() {
+    return this.runOnce(m_io::stop);
   }
 
   public Command idleCoast() {