From dc4fb0e9532756bf410ec6e11fa7eae363326330 Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 31 Mar 2025 22:57:47 -0400 Subject: [PATCH 1/2] Update AutoAlign.java --- .../frc/robot/vision/commands/AutoAlign.java | 240 ++++++++++-------- 1 file changed, 139 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java index e6ce44f..112dff7 100644 --- a/src/main/java/frc/robot/vision/commands/AutoAlign.java +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -1,128 +1,166 @@ package frc.robot.vision.commands; -import static edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition.kRedAllianceWallRightSide; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; 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.Radians; 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 com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric; +import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue; import edu.wpi.first.math.controller.ProfiledPIDController; 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.math.kinematics.ChassisSpeeds; 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.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; 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; - +import java.util.function.Supplier; +/** + * Command to drive to a pose. + */ public class AutoAlign extends Command { - private double x; - private double y; - private LinearVelocity kLineupSpeed = MetersPerSecond.of(.1); - - 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 final Pose2d targetPose; - private static boolean commandRan = false; - - public AutoAlign(Pose2d targetPose) { - //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(); - this.targetPose = targetPose; - - 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()); - - CommandSwerveDrivetrain.getInstance().applyRequest(() -> - RobotContainer.drive.withVelocityX(XOutput) - .withVelocityY(YOutput) - .withRotationalRate(thetaOutput) - - ).execute(); - - commandRan = true; - SmartDashboard.putBoolean("command ran", commandRan); - SmartDashboard.putString("Target Pose X", targetPose.toString()); - SmartDashboard.putBoolean("X at target", xDistanceController.atSetpoint()); - SmartDashboard.putBoolean("Y at target", yDistanceController.atSetpoint()); - SmartDashboard.putBoolean("Theta at target", thetaController.atSetpoint()); - //SmartDashboard.putNumber("TAG ID", tagID); + private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); + private static final Angle THETA_TOLERANCE = Degrees.of(1.0); + + protected static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond), + MAX_ALIGN_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); + protected static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( + MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), + MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); + + private final ProfiledPIDController xController; + private final ProfiledPIDController yController; + private final ProfiledPIDController thetaController; + + private final static CommandSwerveDrivetrain drivetrainSubsystem = CommandSwerveDrivetrain.getInstance(); +// private final FieldCentric fieldCentricSwerveRequest = new FieldCentric() +// .withSteerRequestType(SteerRequestType.MotionMagicExpo) +// .withDriveRequestType(DriveRequestType.Velocity) +// .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Always Blue coordinate system for auto drive + protected final static Supplier poseProvider = () -> PoseEstimatorSubsystem.getInstance().getCurrentPose(); + + /** + * Constructs a DriveToPoseCommand + * + * @param drivetrainSubsystem drivetrain subsystem + * @param goalPose goal pose to drive to + */ + public AutoAlign(Pose2d targetPose) { + this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS); + setGoal(targetPose); + } + + /** + * Constructs a DriveToPoseCommand with specific motion profile constraints + * + * @param drivetrainSubsystem drivetrain subsystem + * @param poseProvider provider to call to get the robot pose + * @param translationConstraints translation motion profile constraints + * @param omegaConstraints rotation motion profile constraints + */ + public AutoAlign( + CommandSwerveDrivetrain drivetrainSubsystem, + Supplier poseProvider, + TrapezoidProfile.Constraints translationConstraints, + TrapezoidProfile.Constraints omegaConstraints) { + + // this.drivetrainSubsystem = CommandSwerveDrivetrain.getInstance(); + // this.poseProvider = () -> PoseEstimatorSubsystem.getInstance().getCurrentPose(); + + xController = new ProfiledPIDController(X_kP, X_kI, X_kD, translationConstraints); + xController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + + yController = new ProfiledPIDController(Y_kP, Y_kI, Y_kD, translationConstraints); + yController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + + thetaController = new ProfiledPIDController(THETA_kP, THETA_kI, THETA_kD, omegaConstraints); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); + + addRequirements(drivetrainSubsystem); + } + + /** + * Sets the goal to drive to. This should be set before the command is scheduled. + * + * @param goalPose goal pose + */ + public void setGoal(Pose2d goalPose) { + thetaController.setGoal(goalPose.getRotation().getRadians()); + xController.setGoal(goalPose.getX()); + yController.setGoal(goalPose.getY()); + } + + @Override + public void initialize() { + var robotPose = poseProvider.get(); + thetaController.reset(robotPose.getRotation().getRadians()); + xController.reset(robotPose.getX()); + yController.reset(robotPose.getY()); + } + + @Override + public void execute() { + var robotPose = poseProvider.get(); + + var xSpeed = xController.calculate(robotPose.getX()); + if (xController.atGoal()) { + xSpeed = 0; } - @Override - public boolean isFinished() { - return xDistanceController.atSetpoint() && yDistanceController.atSetpoint() && thetaController.atSetpoint(); + var ySpeed = yController.calculate(robotPose.getY()); + if (yController.atGoal()) { + ySpeed = 0; } - @Override - public void end(boolean interrupted) { - + var omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); + if (thetaController.atGoal()) { + omegaSpeed = 0; } - + CommandSwerveDrivetrain.getInstance().applyRequest( () -> + RobotContainer.drive.withVelocityX(xSpeed) + .withVelocityY(ySpeed) + .withRotationalRate(omegaSpeed) + ).execute(); + // drivetrainSubsystem.setControl( + // fieldCentricSwerveRequest.withVelocityX(xSpeed).withVelocityY(ySpeed).withRotationalRate(omegaSpeed)); + } + + @Override + public boolean isFinished() { + return xController.atGoal() && yController.atGoal() && thetaController.atGoal(); + } + + @Override + public void end(boolean interrupted) { + drivetrainSubsystem.setControl(new SwerveRequest.Idle()); + } + + private ChassisSpeeds applyLimits(ChassisSpeeds speeds){ + Translation2d translation = new Translation2d(speeds.vxMetersPerSecond, speeds.vyMetersPerSecond); + double magnitude = translation.getNorm(); + if (magnitude < .07) return new ChassisSpeeds(0,0, speeds.omegaRadiansPerSecond); + Rotation2d angle = translation.getAngle(); + magnitude = Math.min(magnitude, MAX_ALIGN_TRANSLATION_VELOCITY.in(MetersPerSecond)); + return new ChassisSpeeds(magnitude * angle.getCos(), magnitude * angle.getSin(), speeds.omegaRadiansPerSecond); } + +} \ No newline at end of file From 2d580b5cc5c02c157010dc7ecf41e1af3aa389ef Mon Sep 17 00:00:00 2001 From: Pix_XL <82770059+PIXLwastaken@users.noreply.github.com> Date: Mon, 31 Mar 2025 23:11:41 -0400 Subject: [PATCH 2/2] fixed dumbahh code --- src/main/java/frc/robot/vision/commands/AutoAlign.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/vision/commands/AutoAlign.java b/src/main/java/frc/robot/vision/commands/AutoAlign.java index 112dff7..a4a2860 100644 --- a/src/main/java/frc/robot/vision/commands/AutoAlign.java +++ b/src/main/java/frc/robot/vision/commands/AutoAlign.java @@ -48,6 +48,9 @@ public class AutoAlign extends Command { private final ProfiledPIDController xController; private final ProfiledPIDController yController; private final ProfiledPIDController thetaController; + double xSpeed; + double ySpeed; + double omegaSpeed; private final static CommandSwerveDrivetrain drivetrainSubsystem = CommandSwerveDrivetrain.getInstance(); // private final FieldCentric fieldCentricSwerveRequest = new FieldCentric() @@ -120,17 +123,17 @@ public void initialize() { public void execute() { var robotPose = poseProvider.get(); - var xSpeed = xController.calculate(robotPose.getX()); + xSpeed = xController.calculate(robotPose.getX()); if (xController.atGoal()) { xSpeed = 0; } - var ySpeed = yController.calculate(robotPose.getY()); + ySpeed = yController.calculate(robotPose.getY()); if (yController.atGoal()) { ySpeed = 0; } - var omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); + omegaSpeed = thetaController.calculate(robotPose.getRotation().getRadians()); if (thetaController.atGoal()) { omegaSpeed = 0; }