diff --git a/src/main/java/frc/robot/constants/TagServoingConstants.java b/src/main/java/frc/robot/constants/TagServoingConstants.java index b4c422e..0061dc8 100644 --- a/src/main/java/frc/robot/constants/TagServoingConstants.java +++ b/src/main/java/frc/robot/constants/TagServoingConstants.java @@ -101,6 +101,8 @@ public class TagServoingConstants { public static final double kCoralStuckAllowence = 0.03; // Reef - public static final Translation2d kBlueReefPose = new Translation2d(4.489323, 4.0259); - public static final Translation2d kRedReefPose = new Translation2d(13.058902, 4.0259); + public static final Translation2d kBlueReefPose = + new Translation2d(4.489323, 4.0259); // welded: 4.489323, 4.0259, AM: 4.489323, 4.02082 + public static final Translation2d kRedReefPose = + new Translation2d(13.058902, 4.0259); // welded: 13.058902, 4.0259, AM: 13.058902, 4.02082 } diff --git a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java index 2532d80..7460c17 100644 --- a/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java +++ b/src/main/java/frc/robot/subsystems/auto/AutoSwitch.java @@ -501,6 +501,23 @@ private AutoCommandInterface getAutoCommand(int switchPos) { ScoringLevel.L2, new Pose2d(7.1008875, 4.0509, Rotation2d.fromDegrees(180.0))); } + case 0x18 -> { + return new StealOneAlgeaAutonCommand( + driveSubsystem, + robotStateSubsystem, + algaeSubsystem, + biscuitSubsystem, + coralSubsystem, + elevatorSubsystem, + tagAlignSubsystem, + visionSubsystem, + "startBargeToG", + "GToBarge", + "bargeToOppG", + "OppGToOppBarge", + ScoringLevel.L2, + new Pose2d(7.1, 3.7209, Rotation2d.fromRadians(3.14159))); + } case 0x20 -> { return new ProcessorShallowSlowAutonCommand(