Added fallback for turret

.gitignore

@@ -188,3 +188,4 @@ compile_commands.json
 simgui-ds.json
 
 build-info.txt
+.vscode/settings.json

src/main/java/frc/robot/Controls.java

@@ -254,6 +254,7 @@ private void configureLauncherBindings() {
       return;
     }
 
+    // Shooting triggers
     driverController
         .rightTrigger()
         .or(readyToShoot)
@@ -263,10 +264,10 @@ private void configureLauncherBindings() {
             Commands.parallel(
                     Commands.either(
                         AutoDriveRotate.autoRotate(
-                            s.drivebaseSubsystem,
+                            s,
                             () -> getDriveX(),
                             () -> getDriveY(),
-                            () -> Units.rotationsToDegrees(s.turretSubsystem.getTurretPosition())),
+                            () -> Units.rotationsToRadians(s.turretSubsystem.getTurretPosition())),
                         s.drivebaseSubsystem.applyRequest(
                             () ->
                                 drive
@@ -303,6 +304,43 @@ private void configureLauncherBindings() {
                           s.flywheels.switchSlot(false);
                         }))
                 .withName("Launching Finished"));
+
+    // This should only be run if turret is fixed to 0. Remember to comment out the default command
+    // for turret rotate to target with calc
+    // driverController
+    //     .rightTrigger()
+    //     .whileTrue(
+    //         Commands.parallel(
+    //                 s.launcherSubsystem.launcherAimCommand(),
+    //                 AutoDriveRotate.autoRotate(
+    //                     s, () -> driverController.getLeftX(), () -> driverController.getLeftY()),
+    //                 Commands.waitUntil(() -> s.launcherSubsystem.isAtTargetFallback())
+    //                     .andThen(
+    //                         Commands.parallel(
+    //                                 Commands.runOnce(() -> s.flywheels.switchSlot(true)),
+    //                                 s.indexerSubsystem.runIndexer(),
+    //                                 Commands.runOnce(() -> ledsMode = LEDMode.LAUNCH),
+    //                                 Commands.waitSeconds(1)
+    //                                     .andThen(Commands.runOnce(() -> updateIntakeMode())))
+    //                             .onlyWhile(() -> s.launcherSubsystem.isAtTarget())
+    //                             .andThen(
+    //                                 Commands.runOnce(
+    //                                     () -> {
+    //                                       updateIntakeMode();
+    //                                       ledsMode = LEDMode.LAUNCHING;
+    //                                     })))
+    //                     .repeatedly())
+    //             .withName("Launching Command"))
+    //     .onFalse(
+    //         s.launcherSubsystem
+    //             .rawStowCommand()
+    //             .alongWith(
+    //                 Commands.runOnce(
+    //                     () -> {
+    //                       updateIntakeMode();
+    //                       s.flywheels.switchSlot(false);
+    //                     }))
+    //             .withName("Launching Finished"));
     driverController
         .start()
         .onTrue(

src/main/java/frc/robot/subsystems/auto/AutoDriveRotate.java

@@ -1,97 +1,95 @@
 package frc.robot.subsystems.auto;
 
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.subsystems.drivebase.CommandSwerveDrivetrain;
-import frc.robot.util.AllianceUtils;
-import frc.robot.util.tuning.LauncherConstants;
+import frc.robot.Subsystems;
+import frc.robot.subsystems.launcher.LaunchCalculator;
 import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
 
 public class AutoDriveRotate {
   public static Command autoRotate(
-      CommandSwerveDrivetrain drivebaseSubsystem,
-      DoubleSupplier xSupplier,
-      DoubleSupplier ySupplier,
-      DoubleSupplier turretOffsetDegrees) {
-    return new AutoRotateCommand(drivebaseSubsystem, xSupplier, ySupplier, turretOffsetDegrees)
-        .withName("Auto Align");
+      Subsystems s, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier turretPos) {
+    return new AutoRotateCommand(s, xSupplier, ySupplier, turretPos).withName("Auto Align");
   }
 
   // Tunable:
-  private static final double SPEED_LIMIT = 2 * Math.PI; // Radians / second
+  private static final double SPEED_LIMIT = 4 * Math.PI; // Radians / second
   private static final double TOLERANCE = Math.toRadians(3);
   private static final double VELOCITY_TOLERANCE = Math.toRadians(5);
   private static final double kP = 8.0;
   private static final double kI = 0.0;
   private static final double kD = 0.0;
 
+  // Launch Calculator Instance
+  private static final LaunchCalculator launchCalc = LaunchCalculator.getInstance();
+
   private static class AutoRotateCommand extends Command {
     protected final PIDController pidRotate = new PIDController(kP, kI, kD);
 
-    protected final CommandSwerveDrivetrain drive;
-    protected Translation2d targetTranslation;
+    protected final Subsystems s;
+    private final Supplier<Rotation2d> targetSupplier;
     private final DoubleSupplier xSupplier;
     private final DoubleSupplier ySupplier;
-    private final DoubleSupplier turretOffsetDegrees;
     private final DoublePublisher anglePub;
-
+    private final DoubleSupplier turretPos;
     private final SwerveRequest.FieldCentric driveRequest =
         new SwerveRequest.FieldCentric().withDriveRequestType(DriveRequestType.OpenLoopVoltage);
 
     public AutoRotateCommand(
-        CommandSwerveDrivetrain drive,
+        Subsystems s,
         DoubleSupplier xSupplier,
         DoubleSupplier ySupplier,
-        DoubleSupplier turretOffsetDegrees) {
-      this.drive = drive;
+        DoubleSupplier turretPos) {
+      this.turretPos = turretPos;
+      targetSupplier =
+          () -> launchCalc.getParameters(s.drivebaseSubsystem, s.turretSubsystem).targetTurret();
+      this.s = s;
       this.xSupplier = xSupplier;
       this.ySupplier = ySupplier;
-      this.turretOffsetDegrees = turretOffsetDegrees;
       anglePub =
           NetworkTableInstance.getDefault().getDoubleTopic("/drivebase/targetRotation").publish();
       pidRotate.enableContinuousInput(-Math.PI, Math.PI);
       pidRotate.setTolerance(TOLERANCE, VELOCITY_TOLERANCE);
       setName("Auto Align");
-      addRequirements(drive);
-    }
-
-    @Override
-    public void initialize() {
-      targetTranslation = AllianceUtils.getHubTranslation2d();
+      addRequirements(s.drivebaseSubsystem);
     }
 
     @Override
     public void execute() {
-      Pose2d currentPose = drive.getState().Pose;
-      Pose2d turretPose = currentPose.transformBy(LauncherConstants.turretTransform());
-      Translation2d toTarget = targetTranslation.minus(turretPose.getTranslation());
-      // The launcher faces the back of the robot so Math.PI is added to align the back of the robot
-      Rotation2d targetRotate =
-          new Rotation2d(
-              Math.atan2(toTarget.getY(), toTarget.getX())
-                  + Math.PI
-                  + Math.toRadians(turretOffsetDegrees.getAsDouble()));
+      if (s.turretSubsystem == null) return;
+
+      SwerveDriveState currentState = s.drivebaseSubsystem.getState();
+      Rotation2d currentRobotRotation = currentState.Pose.getRotation();
+
+      Rotation2d robotRelativeTarget = targetSupplier.get();
+
+      // Added math.pi so that facing forward for turret is facing intake
+      double fieldRelativeSetpoint =
+          currentRobotRotation.plus(robotRelativeTarget).getRadians()
+              + turretPos.getAsDouble()
+              + Math.PI;
+
       double rotationOutput =
-          pidRotate.calculate(
-              drive.getState().Pose.getRotation().getRadians(), targetRotate.getRadians());
+          pidRotate.calculate(currentRobotRotation.getRadians(), fieldRelativeSetpoint);
+
       rotationOutput = MathUtil.clamp(rotationOutput, -SPEED_LIMIT, SPEED_LIMIT);
-      anglePub.set(targetRotate.getDegrees());
+      anglePub.set(fieldRelativeSetpoint);
       SwerveRequest request =
           driveRequest
               .withVelocityX(xSupplier.getAsDouble())
               .withVelocityY(ySupplier.getAsDouble())
               .withRotationalRate(rotationOutput);
       // Set the drive control with the created request
-      drive.setControl(request);
+      s.drivebaseSubsystem.setControl(request);
     }
 
     @Override
@@ -107,7 +105,7 @@ public void end(boolean interrupted) {
       // Create a swerve request to stop all motion by setting velocities and rotational rate to 0
       SwerveRequest stop = driveRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0);
       // Set the drive control with the stop request to halt all movement
-      drive.setControl(stop);
+      s.drivebaseSubsystem.setControl(stop);
     }
   }
 }

src/main/java/frc/robot/subsystems/auto/AutoLogic.java

@@ -229,8 +229,7 @@ public static Command launcherCommand() {
 
   public static Command launcherSimCommand() {
     return Commands.sequence(
-        AutoDriveRotate.autoRotate(
-            s.drivebaseSubsystem, () -> 0, () -> 0, () -> 0), // SIM PURPOSES ONLY
+        AutoDriveRotate.autoRotate(s, () -> 0, () -> 0, () -> 0), // SIM PURPOSES ONLY
         Commands.run(
                 () ->
                     FuelSim.getInstance()

src/main/java/frc/robot/subsystems/launcher/LauncherSubsystem.java

@@ -1,14 +1,12 @@
 package frc.robot.subsystems.launcher;
 
-import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Subsystems;
 import frc.robot.subsystems.launcher.LaunchCalculator.LaunchingParameters;
-import frc.robot.util.tuning.LauncherConstants;
 
 public class LauncherSubsystem extends SubsystemBase {
   protected double flywheelsGoal;
@@ -50,13 +48,21 @@ public boolean isAtTarget() {
     if (launchParameters == null) {
       return false;
     }
-    SwerveDriveState driveState = s.drivebaseSubsystem.getState();
     return s.flywheels.atTargetVelocity(flywheelsGoal, s.flywheels.FLYWHEEL_TOLERANCE)
         && s.hood.atTargetPosition()
-        && s.turretSubsystem.atTarget()
-        && !LaunchCalculator.isApproachingTrench(driveState.Pose, driveState.Speeds)
-        && !LaunchCalculator.isUnderClimb(
-            driveState.Pose.transformBy(LauncherConstants.turretTransform()));
+        && s.turretSubsystem.atTarget(TurretSubsystem.TURRET_DEGREE_TOLERANCE)
+        && !LaunchCalculator.isApproachingTrench(
+            s.drivebaseSubsystem.getState().Pose, s.drivebaseSubsystem.getState().Speeds);
+  }
+
+  public boolean isAtTargetFallback() {
+    if (launchParameters == null) {
+      return false;
+    }
+    return s.flywheels.atTargetVelocity(flywheelsGoal, s.flywheels.FLYWHEEL_TOLERANCE)
+        && s.hood.atTargetPosition()
+        && !LaunchCalculator.isCloseToTrench(launchParameters.turretPose())
+        && s.turretSubsystem.atTarget(TurretSubsystem.TURRET_DEGREE_TOLERANCE_FALLBACK);
   }
 
   public boolean isHoodAtTarget() {

src/main/java/frc/robot/subsystems/launcher/TurretSubsystem.java

@@ -46,7 +46,9 @@ public class TurretSubsystem extends SubsystemBase {
   // The tolerance is this high because the turret position is always updating so it is not
   // always exactly where it should be, I am mainly using this to stop shooting when the
   // turret hits its wraparound point
-  public static final double TURRET_DEGREE_TOLERANCE = 20;
+  public static final double TURRET_DEGREE_TOLERANCE = 12;
+
+  public static final double TURRET_DEGREE_TOLERANCE_FALLBACK = 15;
 
   // Positions
   private double targetPos;
@@ -230,9 +232,9 @@ public double getTurretPosition() {
     return turretMotor.getPosition().getValueAsDouble();
   }
 
-  public boolean atTarget() {
+  public boolean atTarget(double tolerance) {
     return Math.abs(turretMotor.getPosition().getValueAsDouble() - targetPos)
-        < Units.degreesToRotations(TURRET_DEGREE_TOLERANCE);
+        < Units.degreesToRotations(tolerance);
   }
 
   /**