diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2c51cc7..b04c2cb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 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 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 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 RAM_SS_RIGHT_SEQUENCE = List.of( + new Pose2d(7.797,2.168,Rotation2d.kCW_90deg), + new Pose2d(7.829,3.650,Rotation2d.kCW_90deg) + ); + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e402363..8f9280c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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(); } diff --git a/src/main/java/frc/robot/commands/ShootPreloadCommand.java b/src/main/java/frc/robot/commands/ShootPreloadCommand.java index 8bde4fd..ac24416 100644 --- a/src/main/java/frc/robot/commands/ShootPreloadCommand.java +++ b/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( diff --git a/src/main/java/frc/robot/util/auto/IntakeStrategy.java b/src/main/java/frc/robot/util/auto/IntakeStrategy.java index 5a9fab8..d900b71 100644 --- a/src/main/java/frc/robot/util/auto/IntakeStrategy.java +++ b/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 }