Lob shot align working

FRC2713 · Apr 3, 2024 · df47a1d · df47a1d
1 parent f2d8827
commit df47a1d
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 2 deletions.
diff --git 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
@@ -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
@@ -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);