Mukwonago2026 merge into dev

src/main/java/frc/robot/Constants.java

@@ -58,7 +58,7 @@ public static class ErrorSettings {
             public static final int TURRET_OFFSET_DECIMAL_PLACE = 1;
             public static final double HOOD_OFFSET_INCREASE = 0.1;
             public static final int HOOD_OFFSET_DECIMAL_PLACE = 1;
-            public static final double SHOOTER_PERCENT_INCREASE = 0.2; // Percent to increase per tick
+            public static final double SHOOTER_PERCENT_INCREASE = .5; // Percent to increase per tick
             public static final int SHOOTER_PERCENT_DECIMAL_PLACE = 1;
 
             public static final double SHOOTER_PERCENT_DEFAULT = 100.0;
@@ -98,6 +98,19 @@ public static class Auto {
                 new Pose2d(7.730, 6.457, Rotation2d.fromDegrees(-160))
             );
 
+            public static final List<Pose2d> BACK_N_FORTH_RIGHT_SEQUENCE = List.of(
+                new Pose2d(8.500, 2.1, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-45))
+            );
+
+
+            public static final List<Pose2d> BACK_N_FORTH_LEFT_SEQUENCE = List.of(
+                new Pose2d(8.500, 6.000, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.154, 1.349, Rotation2d.fromDegrees(-45)),
+                new Pose2d(8.306, 6.707, Rotation2d.fromDegrees(-45))
+            );
+
             public static final Translation2d NEUTRAL_RIGHT_SPIN_TRANSLATION = new Translation2d(
                 7.730,
                 7.457
@@ -120,6 +133,16 @@ public static class Auto {
                 new Pose2d(7.829,4.051,Rotation2d.kCW_90deg)
             );
 
+            public static final List<Pose2d> RAM_SS_LEFT_SEQUENCE = List.of(
+                new Pose2d(7.797,5.902,Rotation2d.kCW_90deg),
+                new Pose2d(7.829,4.400,Rotation2d.kCW_90deg)
+            );
+
+            public static final List<Pose2d> RAM_SS_RIGHT_SEQUENCE = List.of(
+                new Pose2d(7.797,2.168,Rotation2d.kCW_90deg),
+                new Pose2d(7.829,3.650,Rotation2d.kCW_90deg)
+            );
+
 
         }
 

src/main/java/frc/robot/RobotContainer.java

@@ -284,7 +284,7 @@ public void configureBindings() {
     Trigger reverseTransferTrigger = new Trigger(() -> driverController.getXButton());
 
     // Driver X button: hold to lock robot pose (X-lock)
-    Trigger xLockTrigger = new Trigger(() -> operatorController.getXButton());
+    Trigger xLockTrigger = new Trigger(() -> driverController.getXButton());
     xLockTrigger.whileTrue(driveSubsystem.getLockPoseCommand());
 
     /* Shooter runs while button held */
@@ -322,7 +322,7 @@ public void configureBindings() {
             intakeRoller.getStopCommand()));
 
     reverseTransferTrigger.whileTrue(
-      transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
+       transferSubsystem.getSetPowerCommand(-1)).onFalse(transferSubsystem.getSetPowerCommand(0));
 
     // =========================
     // Turret Offset Adjustment (POV Left / Right)
@@ -388,6 +388,26 @@ public void configureBindings() {
             shooterPercent.getHeldIntervalCommand(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_INCREASE,
                 Constants.Operator.ErrorSettings.SETTINGS_DELAY_TIME));
 
+    // ==============================
+    // Turret / Hood Offset Reset (Left Stick Click / Right Stick Click)
+    // ==============================
+
+    // Left Stick Click : Reset turret offset to zero
+    new Trigger(() -> operatorController.getLeftStickButton())
+        .onTrue(new InstantCommand(() -> turretOffsetDegrees.set(0.0)));
+
+    // Right Stick Click : Reset hood offset to zero
+    new Trigger(() -> operatorController.getRightStickButton())
+        .onTrue(new InstantCommand(() -> hoodOffsetDegrees.set(0.0)));
+
+    // ==============================
+    // Shooter Percent Reset (Left Trigger / Right Trigger)
+    // ==============================
+
+    // Either Trigger : Reset shooter percent to default
+    new Trigger(() -> operatorController.getLeftTriggerAxis() > 0.5
+        || operatorController.getRightTriggerAxis() > 0.5)
+        .onTrue(new InstantCommand(() -> shooterPercent.set(Constants.Operator.ErrorSettings.SHOOTER_PERCENT_DEFAULT)));
     new Trigger(
         () -> operatorController.getAButton()).onTrue(
             new InstantCommand(() -> selectedFixedTarget = FixedTarget.A));
@@ -587,8 +607,8 @@ private Command getIntakeStrategyCommand() {
             turretSubsystem,
             transferSubsystem,
             () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION),
-            Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE,
-            Constants.Operator.Auto.DEPOT_INTAKE_POSE,
+            FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_READY_INTAKE_POSE),
+            FieldUtil.flipIfRed(Constants.Operator.Auto.DEPOT_INTAKE_POSE),
             false,
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));
@@ -602,8 +622,8 @@ private Command getIntakeStrategyCommand() {
             turretSubsystem,
             transferSubsystem,
             () -> FieldUtil.flipIfRed(Constants.Field.BLUE_HUB_TRANSLATION),
-            Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE,
-            Constants.Operator.Auto.OUTPOST_INTAKE_POSE,
+            FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_READY_INTAKE_POSE),
+            FieldUtil.flipIfRed(Constants.Operator.Auto.OUTPOST_INTAKE_POSE),
             false,
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));
@@ -662,6 +682,16 @@ private Command getIntakeStrategyCommand() {
             .map(FieldUtil::flipIfRed)
             .toList());
 
+      case RAM_SS_L:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.RAM_SS_LEFT_SEQUENCE.stream()
+            .map(FieldUtil::flipIfRed)
+            .toList());
+
+      case RAM_SS_R:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.RAM_SS_RIGHT_SEQUENCE.stream()
+            .map(FieldUtil::flipIfRed)
+            .toList());
+
       case CUSTOM:
 
         Pose2d ready = getDashboardPose("Auto/CustomReadyPose");
@@ -684,6 +714,14 @@ private Command getIntakeStrategyCommand() {
             Constants.Operator.Auto.AUTO_INTAKE_MAX_SPEED,
             SmartDashboard.getNumber("Auto/IntakeShootTime", Constants.Operator.Auto.DEFAULT_INTAKE_SHOOT_TIME));
 
+      case B_N_F_R:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_RIGHT_SEQUENCE.stream()
+              .map(FieldUtil::flipIfRed)
+              .toList());
+      case B_N_F_L:
+        return new DriveToSequenceCommand(driveSubsystem, Constants.Operator.Auto.BACK_N_FORTH_LEFT_SEQUENCE.stream()
+              .map(FieldUtil::flipIfRed)
+              .toList());
       default:
         return Commands.none();
     }

src/main/java/frc/robot/commands/ShootPreloadCommand.java

@@ -5,10 +5,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.subsystems.IntakePivotSubsystem;
 import frc.robot.subsystems.IntakeRollerSubsystem;
 import frc.robot.subsystems.ShooterSubsystem;
@@ -32,12 +32,19 @@ public ShootPreloadCommand(
         addCommands(
 
             Commands.deadline(
-                Commands.waitSeconds(shootPreloadTime),
-                turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
-                shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
-                transfer.getLoadCommand(),
-                intakePivot.deployIntakeCommand(),
-                intakeRoller.getIntakeCommand()
+                Commands.waitSeconds(shootPreloadTime + 2),
+                Commands.parallel(
+                    new ParallelCommandGroup(
+                        turret.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
+                        shooter.getTargetCommand(targetSupplier, robotPoseSupplier, robotVelocitySupplier),
+                        intakeRoller.getIntakeCommand(),
+                        intakePivot.deployIntakeCommand()
+                    ),
+                    Commands.sequence(
+                        new WaitCommand(2),
+                        transfer.getLoadCommand()
+                    )
+                )
             ),
 
             Commands.parallel(

src/main/java/frc/robot/util/auto/IntakeStrategy.java

@@ -9,6 +9,10 @@ public enum IntakeStrategy {
     RAM_LEFT,
     RAM_RIGHT,
     JUST_SHOOT,
+    B_N_F_L,
+    B_N_F_R,
+    RAM_SS_L,
+    RAM_SS_R,
     CUSTOM
 
 }