From 24dd1e42f666c04df07ebe910165f43b457fd6d7 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Sun, 23 Mar 2025 10:26:44 -0400 Subject: [PATCH] Auto Align V2 ts pmo bro --- .../java/frc/robot/vision/VisionConfig.java | 123 +++++----- .../frc/robot/vision/commands/AutoAlign.java | 224 ++++++++++++++++++ .../robot/vision/commands/LineupCommand.java | 8 +- 3 files changed, 286 insertions(+), 69 deletions(-) create mode 100644 src/main/java/frc/robot/vision/commands/AutoAlign.java diff --git a/src/main/java/frc/robot/vision/VisionConfig.java b/src/main/java/frc/robot/vision/VisionConfig.java index fbf0007..f737cd3 100644 --- a/src/main/java/frc/robot/vision/VisionConfig.java +++ b/src/main/java/frc/robot/vision/VisionConfig.java @@ -121,7 +121,64 @@ public class AlignmentConfig { public static final AngularVelocity MAX_ALIGN_ANGULAR_VELOCITY = RotationsPerSecond.of(1.25).times(0.75); public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond.of(6.0 * Math.PI); - //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW + //April Tag IDs + public static final double id1 = 1; + public static final double id2 = 2; + public static final double id3 = 3; + public static final double id4 = 4; + public static final double id5 = 5; + public static final double id6 = 6; + public static final double id7 = 7; + public static final double id8 = 8; + public static final double id9 = 9; + public static final double id10 = 10; + public static final double id11 = 11; + public static final double id12 = 12; + public static final double id13 = 13; + public static final double id14 = 14; + public static final double id15 = 15; + public static final double id16 = 16; + public static final double id17 = 17; + public static final double id18 = 18; + public static final double id19 = 19; + public static final double id20 = 20; + public static final double id21 = 21; + public static final double id22 = 22; + + //Joey's Pose Constants for the Reef Locations + public static final Pose2d Error = new Pose2d(6, 6, Rotation2d.fromDegrees(0)); + + public static final Pose2d Ablue = new Pose2d(3.180, 4.175, Rotation2d.fromDegrees(0)); + public static final Pose2d Bblue = new Pose2d(3.180, 3.850, Rotation2d.fromDegrees(0)); + public static final Pose2d Cblue = new Pose2d(3.685, 2.975, Rotation2d.fromDegrees(60)); + public static final Pose2d Dblue = new Pose2d(3.975, 2.825, Rotation2d.fromDegrees(60)); + public static final Pose2d Eblue = new Pose2d(5.000, 2.825, Rotation2d.fromDegrees(120)); + public static final Pose2d Fblue = new Pose2d(5.285, 2.975, Rotation2d.fromDegrees(120)); + public static final Pose2d Gblue = new Pose2d(5.8, 3.850, Rotation2d.fromDegrees(180)); + public static final Pose2d Hblue = new Pose2d(5.8, 4.175, Rotation2d.fromDegrees(180)); + public static final Pose2d Iblue = new Pose2d(5.285, 5.075, Rotation2d.fromDegrees(240)); + public static final Pose2d Jblue = new Pose2d(5.000, 5.230, Rotation2d.fromDegrees(240)); + public static final Pose2d Kblue = new Pose2d(3.975, 5.230, Rotation2d.fromDegrees(300)); + public static final Pose2d Lblue = new Pose2d(3.685, 5.075, Rotation2d.fromDegrees(300)); + + public static final double fieldFlip = 17.5; + public static final double fieldFlipy = 8; + + public static final Pose2d Ared = new Pose2d(fieldFlip - 3.180, fieldFlipy - 4.175, Rotation2d.fromDegrees(180)); + public static final Pose2d Bred = new Pose2d(fieldFlip - 3.180, fieldFlipy - 3.850, Rotation2d.fromDegrees(180)); + public static final Pose2d Cred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 2.975, Rotation2d.fromDegrees(-120)); + public static final Pose2d Dred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 2.825, Rotation2d.fromDegrees(-120)); + public static final Pose2d Ered = new Pose2d(fieldFlip - 5.000, fieldFlipy - 2.825, Rotation2d.fromDegrees(-60)); + public static final Pose2d Fred = new Pose2d(fieldFlip - 5.285, fieldFlipy - 2.975, Rotation2d.fromDegrees(-60)); + public static final Pose2d Gred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 3.850, Rotation2d.fromDegrees(0)); + public static final Pose2d Hred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 4.175, Rotation2d.fromDegrees(0)); + public static final Pose2d Ired = new Pose2d(fieldFlip - 5.285, fieldFlipy - 5.075, Rotation2d.fromDegrees(-300)); + public static final Pose2d Jred = new Pose2d(fieldFlip - 5.000, fieldFlipy - 5.230, Rotation2d.fromDegrees(-300)); + public static final Pose2d Kred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 5.230, Rotation2d.fromDegrees(-240)); + public static final Pose2d Lred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 5.075, Rotation2d.fromDegrees(-240)); + + + //TODO: MIGHT NEED TO PLAY AROUND WITH ALL THE POSES BELOW // /** Pose of the robot relative to a reef branch for scoring coral on L4 */ // public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L4 = new Transform2d( @@ -324,69 +381,5 @@ public class AlignmentConfig { // public static final Distance LATERAL_TARGET_L4_LEFT = Meters.of(0.05); // public static final Distance LATERAL_TARGET_L4_RIGHT = Meters.of(0.02); - - //April Tag IDs - public static final double id1 = 1; - public static final double id2 = 2; - public static final double id3 = 3; - public static final double id4 = 4; - public static final double id5 = 5; - public static final double id6 = 6; - public static final double id7 = 7; - public static final double id8 = 8; - public static final double id9 = 9; - public static final double id10 = 10; - public static final double id11 = 11; - public static final double id12 = 12; - public static final double id13 = 13; - public static final double id14 = 14; - public static final double id15 = 15; - public static final double id16 = 16; - public static final double id17 = 17; - public static final double id18 = 18; - public static final double id19 = 19; - public static final double id20 = 20; - public static final double id21 = 21; - public static final double id22 = 22; - - - - - //Joey's Pose Constants for the Reef Locations - public static final Pose2d Error = new Pose2d(6, 6, Rotation2d.fromDegrees(0)); - - public static final Pose2d Ablue = new Pose2d(3.180, 4.175, Rotation2d.fromDegrees(0)); - public static final Pose2d Bblue = new Pose2d(3.180, 3.850, Rotation2d.fromDegrees(0)); - public static final Pose2d Cblue = new Pose2d(3.685, 2.975, Rotation2d.fromDegrees(60)); - public static final Pose2d Dblue = new Pose2d(3.975, 2.825, Rotation2d.fromDegrees(60)); - public static final Pose2d Eblue = new Pose2d(5.000, 2.825, Rotation2d.fromDegrees(120)); - public static final Pose2d Fblue = new Pose2d(5.285, 2.975, Rotation2d.fromDegrees(120)); - public static final Pose2d Gblue = new Pose2d(5.8, 3.850, Rotation2d.fromDegrees(180)); - public static final Pose2d Hblue = new Pose2d(5.8, 4.175, Rotation2d.fromDegrees(180)); - public static final Pose2d Iblue = new Pose2d(5.285, 5.075, Rotation2d.fromDegrees(240)); - public static final Pose2d Jblue = new Pose2d(5.000, 5.230, Rotation2d.fromDegrees(240)); - public static final Pose2d Kblue = new Pose2d(3.975, 5.230, Rotation2d.fromDegrees(300)); - public static final Pose2d Lblue = new Pose2d(3.685, 5.075, Rotation2d.fromDegrees(300)); - - public static final double fieldFlip = 17.5; - public static final double fieldFlipy = 8; - - public static final Pose2d Ared = new Pose2d(fieldFlip - 3.180, fieldFlipy - 4.175, Rotation2d.fromDegrees(180)); - public static final Pose2d Bred = new Pose2d(fieldFlip - 3.180, fieldFlipy - 3.850, Rotation2d.fromDegrees(180)); - public static final Pose2d Cred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 2.975, Rotation2d.fromDegrees(-120)); - public static final Pose2d Dred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 2.825, Rotation2d.fromDegrees(-120)); - public static final Pose2d Ered = new Pose2d(fieldFlip - 5.000, fieldFlipy - 2.825, Rotation2d.fromDegrees(-60)); - public static final Pose2d Fred = new Pose2d(fieldFlip - 5.285, fieldFlipy - 2.975, Rotation2d.fromDegrees(-60)); - public static final Pose2d Gred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 3.850, Rotation2d.fromDegrees(0)); - public static final Pose2d Hred = new Pose2d(fieldFlip - 5.8, fieldFlipy - 4.175, Rotation2d.fromDegrees(0)); - public static final Pose2d Ired = new Pose2d(fieldFlip - 5.285, fieldFlipy - 5.075, Rotation2d.fromDegrees(-300)); - public static final Pose2d Jred = new Pose2d(fieldFlip - 5.000, fieldFlipy - 5.230, Rotation2d.fromDegrees(-300)); - public static final Pose2d Kred = new Pose2d(fieldFlip - 3.975, fieldFlipy - 5.230, Rotation2d.fromDegrees(-240)); - public static final Pose2d Lred = new Pose2d(fieldFlip - 3.685, fieldFlipy - 5.075, Rotation2d.fromDegrees(-240)); - } - - - - } \ No newline at end of file diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java new file mode 100644 index 0000000..1c6d48a --- /dev/null +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -0,0 +1,224 @@ +package frc.robot.vision.commands; + +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static frc.robot.vision.VisionConfig.*; +import static frc.robot.vision.VisionConfig.AlignmentConfig.*; + +import org.photonvision.PhotonCamera; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotContainer; +import frc.robot.drivetrain.CommandSwerveDrivetrain; +import frc.robot.vision.PoseEstimatorSubsystem; +import frc.robot.vision.VisionConfig; + + + +public class AutoAlign extends Command { + private double x; + private double y; + private LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); + private PhotonCamera leftCam = new PhotonCamera(CAM_NAMES[0]); + private int tagID; + + private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( + VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), + VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); + + private static final TrapezoidProfile.Constraints THETA_CONSTRAINTS = new TrapezoidProfile.Constraints( + VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), + VisionConfig.AlignmentConfig.MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + + + private static final ProfiledPIDController xDistanceController = new ProfiledPIDController( + VisionConfig.AlignmentConfig.X_kP, + VisionConfig.AlignmentConfig.X_kI, + VisionConfig.AlignmentConfig.X_kD, + TRANSLATION_CONSTRAINTS); + + private static final ProfiledPIDController yDistanceController = new ProfiledPIDController( + VisionConfig.AlignmentConfig.Y_kP, + VisionConfig.AlignmentConfig.Y_kI, + VisionConfig.AlignmentConfig.Y_kD, + TRANSLATION_CONSTRAINTS); + + private static final ProfiledPIDController thetaController = new ProfiledPIDController( + VisionConfig.AlignmentConfig.THETA_kP, + VisionConfig.AlignmentConfig.THETA_kI, + VisionConfig.AlignmentConfig.THETA_kD, + THETA_CONSTRAINTS); + + private static Pose2d currentPose; + private static Pose2d targetPose; + + public AutoAlign(boolean left) { + //set tolerances of all PID controllers + xDistanceController.setTolerance(VisionConfig.AlignmentConfig.DISTANCE_TOLERANCE.in(Meters)); + xDistanceController.setIntegratorRange(-.15, .15); + yDistanceController.setTolerance(VisionConfig.AlignmentConfig.LATERAL_TOLERANCE.in(Meters)); + yDistanceController.setIntegratorRange(-.15, .15); + thetaController.enableContinuousInput(Units.degreesToRadians(-180), Units.degreesToRadians(180)); + + currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); + targetPose = getTargetReefPose(left); + + xDistanceController.reset(0); + yDistanceController.reset(0); + thetaController.reset(0); + + addRequirements(CommandSwerveDrivetrain.getInstance()); + } + + @Override + public void execute() { + currentPose = PoseEstimatorSubsystem.getInstance().getCurrentPose(); + // ChassisSpeeds currentSpeeds = CommandSwerveDrivetrain.getInstance().getState().Speeds; + x = currentPose.getTranslation().getX(); + y = currentPose.getTranslation().getY(); + // double Xvel = currentSpeeds.vxMetersPerSecond; + // double Yvel = currentSpeeds.vyMetersPerSecond; + double XOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(x, targetPose.getX()); + double YOutput = 0.15 * kLineupSpeed.in(MetersPerSecond) * 0.95 * xDistanceController.calculate(y, targetPose.getY()); + double thetaOutput = 0.15 * RotationsPerSecond.of(0.75).in(RadiansPerSecond) * thetaController.calculate(currentPose.getRotation().getRadians(), targetPose.getRotation().getRadians()); + + if (DriverStation.getAlliance().get() == Alliance.Blue){ + CommandSwerveDrivetrain.getInstance().applyRequest(() -> + RobotContainer.drive.withVelocityX(-XOutput) + .withVelocityY(-YOutput) + .withRotationalRate(thetaOutput) + + ).execute(); + } else { + CommandSwerveDrivetrain.getInstance().applyRequest(() -> + RobotContainer.drive.withVelocityX(XOutput) + .withVelocityY(YOutput) + .withRotationalRate(-thetaOutput) + ).execute(); + } + + SmartDashboard.putString("Target Pose X", targetPose.toString()); + + } + + @Override + public boolean isFinished() { + return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); + } + + @Override + public void end(boolean interrupted) { + + } + + private Pose2d getTargetReefPose(boolean left){ + var photonResult = leftCam.getAllUnreadResults(); + var currAlliance = DriverStation.getAlliance(); + + for (var result : photonResult){ + if (result.hasTargets()){ + tagID = result.getBestTarget().fiducialId; + } + } + + if (currAlliance.get() == Alliance.Blue){ + + if (tagID == 17){ + if (left) { + return Cblue; + } else if (!left) { + return Dblue; + } + } else if (tagID == 18) { + if (left) { + return Ablue; + } else if (!left) { + return Bblue; + } + } else if (tagID == 19) { + if (left) { + return Kblue; + } else if (!left) { + return Lblue; + } + } else if (tagID == 20) { + if (left) { + return Iblue; + } else if (!left) { + return Jblue; + } + } else if (tagID == 21) { + if (left) { + return Gblue; + } else if (!left) { + return Hblue; + } + } else if (tagID == 22) { + if (left) { + return Eblue; + } else if (!left) { + return Fblue; + } + } + + } else if (currAlliance.get() == Alliance.Red){ + if (tagID == 7){ + if (left) { + return Ared; + } else if (!left) { + return Bred; + } + } else if (tagID == 8) { + if (left) { + return Cred; + } else if (!left) { + return Dred; + } + } else if (tagID == 9) { + if (left) { + return Ered; + } else if (!left) { + return Fred; + } + } else if (tagID == 10) { + if (left) { + return Gred; + } else if (!left) { + return Hred; + } + } else if (tagID == 11) { + if (left) { + return Ired; + } else if (!left) { + return Jred; + } + } else if (tagID == 6) { + if (left) { + return Kred; + } else if (!left) { + return Lred; + } + } + + } else if (currAlliance.get() != Alliance.Red && currAlliance.get() != Alliance.Blue){ + System.out.println("ERROR: There's no such thing as purple alliance"); + } + + System.out.println("Error: no suitable target pose found (Lineup Command)"); + return currentPose; + + } + + +} diff --git a/src/main/java/frc/robot/vision/commands/LineupCommand.java b/src/main/java/frc/robot/vision/commands/LineupCommand.java index df17f41..2c4f937 100644 --- a/src/main/java/frc/robot/vision/commands/LineupCommand.java +++ b/src/main/java/frc/robot/vision/commands/LineupCommand.java @@ -35,10 +35,10 @@ public class LineupCommand extends Command { - double x; - double y; - double theta; - LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); + double x; + double y; + double theta; + LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( VisionConfig.AlignmentConfig.MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond),