Merge branch 'main' into port-forwarding-insert-question-mark-here

Author: Ishan1522

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

@@ -337,6 +337,18 @@ or the reef closer to the robots (Red side) and goes clockwise. Now thinking abo
             Units.inchesToMeters(642.903),
             Units.inchesToMeters(86.50),
             new Rotation2d(Units.degreesToRadians(180)));
+
+    public static final double RED_REEF_PLACE_X = 2;
+    public static final double RED_REEF_PLACE_Y = 2;
+    public static final Rotation2d RED_REEF_ROTATION = Rotation2d.fromDegrees(-90);
+
+    public static final double RED_PROCESSOR_PLACE_X = 0;
+    public static final double RED_PROCESSOR_PLACE_Y = 0;
+    public static final Rotation2d RED_PROCESSOR_ROTATION = Rotation2d.fromDegrees(-90);
+
+    public static final double RED_FEEDER_STATION_PLACE_X = 0;
+    public static final double RED_FEEDER_STATION_PLACE_Y = 0;
+    public static final Rotation2d RED_FEEDER_STATION_ROTATION = Rotation2d.fromDegrees(-90);
   }
 
   public static final class JoystickConstants {

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

@@ -9,7 +9,9 @@
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.SimulationConstants;
+import frc.robot.commands.autodrive.AutoAlign;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.extras.simulation.field.SimulatedField;
 import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation;
@@ -205,6 +207,11 @@ private void configureButtonBindings() {
     driverLeftDirectionPad.onTrue(
         new InstantCommand(
             () -> swerveDrive.resetEstimatedPose(visionSubsystem.getLastSeenPose())));
+
+    // FieldConstants has all reef poses
+    driverController
+        .a()
+        .whileTrue(new AutoAlign(swerveDrive, visionSubsystem, FieldConstants.RED_REEF_ONE));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/autodrive/AutoAlign.java

@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.commands.autodrive;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import frc.robot.commands.drive.DriveCommandBase;
+import frc.robot.subsystems.swerve.SwerveConstants;
+import frc.robot.subsystems.swerve.SwerveConstants.TrajectoryConstants;
+import frc.robot.subsystems.swerve.SwerveDrive;
+import frc.robot.subsystems.vision.VisionSubsystem;
+
+/* Auto Align takes in Pose2d and moves robot to it */
+public class AutoAlign extends DriveCommandBase {
+
+  private final SwerveDrive swerveDrive;
+  private final VisionSubsystem visionSubsystem;
+
+  private Pose2d targetPose;
+
+  private final ProfiledPIDController rotationController =
+      new ProfiledPIDController(
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_ROTATION_P,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_ROTATION_I,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_ROTATION_D,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_ROTATION_CONSTRAINTS);
+
+  private final ProfiledPIDController xTranslationController =
+      new ProfiledPIDController(
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_P,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_I,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_D,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
+
+  private final ProfiledPIDController yTranslationController =
+      new ProfiledPIDController(
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_P,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_I,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_D,
+          SwerveConstants.TrajectoryConstants.AUTO_LINEUP_TRANSLATION_CONSTRAINTS);
+
+  /**
+   * Creates a new AutoAlign.
+   *
+   * @param visionSubsystem The subsystem for vision
+   * @param swerveDrive The subsystem for the swerve drive
+   * @param targetPose The target pose for the robot to align to
+   */
+  public AutoAlign(SwerveDrive swerveDrive, VisionSubsystem visionSubsystem, Pose2d targetPose) {
+    super(swerveDrive, visionSubsystem);
+    this.targetPose = targetPose;
+    this.swerveDrive = swerveDrive;
+    this.visionSubsystem = visionSubsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(swerveDrive, visionSubsystem);
+    // Enables continuous input for the rotation controller
+    rotationController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    super.execute();
+    // Gets the error between the desired pos (the target) and the current pos of the robot
+    Pose2d drivePose = swerveDrive.getEstimatedPose();
+    double xPoseError = targetPose.getX() - drivePose.getX();
+    double yPoseError = targetPose.getY() - drivePose.getY();
+    double thetaPoseError =
+        targetPose.getRotation().getRadians() - drivePose.getRotation().getRadians();
+
+    // Uses the PID controllers to calculate the drive output
+    double xOutput =
+        MathUtil.applyDeadband(
+            xTranslationController.calculate(xPoseError, 0), TrajectoryConstants.DEADBAND_AMOUNT);
+    double yOutput =
+        MathUtil.applyDeadband(
+            yTranslationController.calculate(yPoseError, 0), TrajectoryConstants.DEADBAND_AMOUNT);
+    double turnOutput =
+        MathUtil.applyDeadband(
+            rotationController.calculate(thetaPoseError, 0), TrajectoryConstants.DEADBAND_AMOUNT);
+
+    // Gets the chassis speeds for the robot using the odometry rotation (not alliance relative)
+    ChassisSpeeds chassisSpeeds =
+        ChassisSpeeds.fromFieldRelativeSpeeds(
+            xOutput, yOutput, turnOutput, swerveDrive.getOdometryRotation2d());
+
+    // Drives the robot towards the target pose
+    swerveDrive.drive(
+        chassisSpeeds.vxMetersPerSecond,
+        chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond,
+        false);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    swerveDrive.drive(0, 0, 0, false);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}