diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d42fe5ce..e28e16d4 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -454,11 +454,13 @@ public void createOperatorBindings() { .b() .whileTrue( Commands.sequence( + Cmds.setState(MotionMode.LOB_SHOT_ALIGN), new WaitUntilCommand(() -> intake.state == Intake.State.OFF), Cmds.setState(ShooterState.LOB_SHOT), Cmds.setState(ShooterPivot.State.LOB_SHOT))) .whileFalse( Commands.sequence( + Cmds.setState(MotionMode.FULL_DRIVE), Cmds.setState(FeederState.FEED_SHOT), new WaitUntilCommand(() -> !Robot.shooter.hasGamePiece()), new WaitCommand(0.1), @@ -728,7 +730,8 @@ public void robotPeriodic() { updatePreMatchDashboardValues(); if (Math.abs(driver.getRightX()) > 0.25 - && swerveDrive.getMotionMode() != MotionMode.DRIVE_TOWARDS_GP) { + && swerveDrive.getMotionMode() != MotionMode.DRIVE_TOWARDS_GP + && swerveDrive.getMotionMode() != MotionMode.LOB_SHOT_ALIGN) { swerveDrive.setMotionMode(MotionMode.FULL_DRIVE); } diff --git a/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java index d43f9f4e..76ae518a 100644 --- a/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerveIO/SwerveSubsystem.java @@ -51,7 +51,8 @@ public enum MotionMode { TRAJECTORY, LOCKDOWN, ALIGN_TO_TAG, - DRIVE_TOWARDS_GP + DRIVE_TOWARDS_GP, + LOB_SHOT_ALIGN } SwerveIO io; @@ -495,6 +496,9 @@ public void periodic() { case ALIGN_TO_TAG: setDesiredChassisSpeeds(MotionHandler.driveAlignToTag()); break; + case LOB_SHOT_ALIGN: + setDesiredChassisSpeeds(MotionHandler.driveLobShotAlign()); + break; case DRIVE_TOWARDS_GP: setDesiredChassisSpeeds(MotionHandler.driveTowardsGP()); break; diff --git a/src/main/java/frc/robot/util/MotionHandler.java b/src/main/java/frc/robot/util/MotionHandler.java index cf8e5b98..1f465775 100644 --- a/src/main/java/frc/robot/util/MotionHandler.java +++ b/src/main/java/frc/robot/util/MotionHandler.java @@ -109,6 +109,13 @@ public static ChassisSpeeds driveAlignToTag() { return driveHeadingController(); } + public static ChassisSpeeds driveLobShotAlign() { + SwerveHeadingController.getInstance() + .setSetpoint(RotateScore.getOptimalAmpAngle(Robot.swerveDrive.getEstimatedPose())); + + return driveHeadingController(); + } + public static ChassisSpeeds driveTowardsGP() { Logger.recordOutput("OTF/DrivingToGP/HasGPLock", VehicleState.getInstance().hasGPLock);