From 53773829d67c98e77f9ccaf8fbf5d74b7d38e713 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Fri, 28 Mar 2025 17:41:49 -0700 Subject: [PATCH 1/2] DriveWithPoseTransformation --- .../OperatorCommandMap.java | 16 ++-- .../drive/commands/DriveToPose.java | 78 +++++++++++++++++++ .../AlignCameraToAprilTagCalculator.java | 37 +++++++++ 3 files changed, 125 insertions(+), 6 deletions(-) create mode 100644 src/main/java/competition/subsystems/drive/commands/DriveToPose.java diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 66e4f37d..b3580cfa 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -27,6 +27,7 @@ import competition.subsystems.drive.commands.CalibrateDriveCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; import competition.subsystems.drive.commands.DriveToNearestReefFaceWithPID; +import competition.subsystems.drive.commands.DriveToPose; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.drive.logic.AlignCameraToAprilTagCalculator; import competition.subsystems.elevator.ElevatorSubsystem; @@ -79,7 +80,8 @@ public void setupDriverCommands( driveToClosestStationCommandGroupFactory, CoprocessorCommunicationSubsystem coprocessorCommunicationSubsystem, PathDriveToLocationForCoralStationFactory pathDriveToLocationForCoralStationFactory, - AlignCameraToAprilTagCalculator.AlignCameraToAprilTagCalculatorFactory aprilTagCalculatorFactory) { + AlignCameraToAprilTagCalculator.AlignCameraToAprilTagCalculatorFactory aprilTagCalculatorFactory, + DriveToPose driveToPose) { resetHeading.setHeadingToApply(0); operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.Start).onTrue(resetHeading); @@ -113,11 +115,13 @@ public void setupDriverCommands( ); SequentialCommandGroup driveToClosestCoralStation = driveToClosestStationCommandGroupFactory.createDriveOnly(true); - operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( - pathDriveToClosestCoralStation, - driveToClosestCoralStation, - () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) - )); +// operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( +// pathDriveToClosestCoralStation, +// driveToClosestCoralStation, +// () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) +// )); + + operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(driveToPose); // operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); // operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPose.java b/src/main/java/competition/subsystems/drive/commands/DriveToPose.java new file mode 100644 index 00000000..74e0906f --- /dev/null +++ b/src/main/java/competition/subsystems/drive/commands/DriveToPose.java @@ -0,0 +1,78 @@ +package competition.subsystems.drive.commands; + +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.pose.Landmarks; +import competition.subsystems.pose.PoseSubsystem; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.util.Units; +import xbot.common.command.BaseCommand; +import xbot.common.math.XYPair; +import xbot.common.subsystems.drive.control_logic.HeadingModule; + +import javax.inject.Inject; + +public class DriveToPose extends BaseCommand { + DriveSubsystem drive; + PoseSubsystem pose; + HeadingModule headingModule; + + @Inject + public DriveToPose(DriveSubsystem drive, PoseSubsystem pose, HeadingModule.HeadingModuleFactory headingModuleFactory) { + this.drive = drive; + this.pose = pose; + this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());; + this.addRequirements(drive); + } + + @Override + public void initialize() { + log.info("Initializing"); + } + + @Override + public void execute() { + var targetPose = PoseSubsystem.convertBlueToRedIfNeeded(pose.getClosestReefFacePose()); + targetPose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae); + var currentPose = pose.getCurrentPose2d(); + + var goal = getDriveTarget(currentPose, targetPose); + + double rotationIntent = headingModule.calculateHeadingPower(goal.getRotation()); + + double dx = drive.getPositionalPid().calculate(goal.getX(), currentPose.getX()); + double dy = drive.getPositionalPid().calculate(goal.getY(), currentPose.getY()); + XYPair driveIntent = new XYPair(dx, dy); + + + drive.fieldOrientedDrive(driveIntent, rotationIntent, pose.getCurrentHeading().getDegrees(), new XYPair()); + aKitLog.record("Goal Pose", goal); + } + + public Pose2d getDriveTarget(Pose2d currentPose, Pose2d targetPose) { + + // Final line up + var reefFaceLength = Units.inchesToMeters(36.792600); + var maxDistanceReefLineUp = 1.5; + var offset = currentPose.relativeTo(targetPose); + double yDistance = Math.abs(offset.getY()); + double xDistance = Math.abs(offset.getX()); + double shiftXT = + MathUtil.clamp( + (yDistance / (reefFaceLength * 2)) + ((xDistance - 0.3) / (reefFaceLength * 3)), + 0.0, + 1.0); + double shiftYT = MathUtil.clamp(offset.getX() / reefFaceLength, 0.0, 1.0); + return targetPose.transformBy( + new Transform2d(-shiftXT * maxDistanceReefLineUp, + Math.copySign(shiftYT * maxDistanceReefLineUp * 1.2, offset.getY()), new Rotation2d(0))); + } + + @Override + public void end(boolean isInterrupted) { + super.end(isInterrupted); + drive.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java index def055f4..95fbc79f 100644 --- a/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java +++ b/src/main/java/competition/subsystems/drive/logic/AlignCameraToAprilTagCalculator.java @@ -10,12 +10,14 @@ import competition.subsystems.vision.AprilTagVisionSubsystemExtended; import dagger.assisted.AssistedFactory; import dagger.assisted.AssistedInject; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; @@ -344,6 +346,22 @@ public AlignCameraToAprilTagAdvice getXYPowersAlignToAprilTag(Pose2d currentPose var vectorTowardsInterstitial = interstitialPoint.getTranslation().minus(currentPose.getTranslation()); var normalizedVector = vectorTowardsInterstitial.div(vectorTowardsInterstitial.getNorm()); driveIntent = new XYPair(normalizedVector.getX(), normalizedVector.getY()).scale(approachSpeedFactor); + +// Pose2d goal = getDriveTarget(currentPose, interstitialPoint); +// double dx = drive.getPositionalPid().calculate(goal.getX(), currentPose.getX()); +// double dy = drive.getPositionalPid().calculate(goal.getY(), currentPose.getY()); +// driveIntent = new XYPair(dx, dy); + + // Calculate the goal position + Pose2d goal = getDriveTarget(currentPose, interstitialPoint); + double dx = goal.getX() - currentPose.getX(); + double dy = goal.getY() - currentPose.getY(); + + // Calculate the velocity vector and scale it + double velocityX = dx * approachSpeedFactor; + double velocityY = dy * approachSpeedFactor; + driveIntent = new XYPair(velocityX, velocityY); + rotationIntent = headingModule.calculateHeadingPower(headingToPointAtAprilTag); // Finally, a check to see if we're quite close and should advance to the next state. @@ -511,4 +529,23 @@ private Translation2d getCoralStationPreShovePoint() { var projectedDelta = new Translation2d(0.25, stationLocation.getRotation()); return stationLocation.getTranslation().plus(projectedDelta); } + + public static Pose2d getDriveTarget(Pose2d currentPose, Pose2d targetPose) { + + // Final line up + var reefFaceLength = Units.inchesToMeters(36.792600); + var maxDistanceReefLineUp = 1.5; + var offset = currentPose.relativeTo(targetPose); + double yDistance = Math.abs(offset.getY()); + double xDistance = Math.abs(offset.getX()); + double shiftXT = + MathUtil.clamp( + (yDistance / (reefFaceLength * 2)) + ((xDistance - 0.3) / (reefFaceLength * 3)), + 0.0, + 1.0); + double shiftYT = MathUtil.clamp(offset.getX() / reefFaceLength, 0.0, 1.0); + return targetPose.transformBy( + new Transform2d(-shiftXT * maxDistanceReefLineUp, + Math.copySign(shiftYT * maxDistanceReefLineUp * 0.8, offset.getY()), new Rotation2d(0))); + } } From 21aca45398b2c166c6a79043d7b8f6fe5ec42fe9 Mon Sep 17 00:00:00 2001 From: kujo27 Date: Sat, 29 Mar 2025 15:39:50 -0700 Subject: [PATCH 2/2] clean --- .../OperatorCommandMap.java | 15 ++-- .../AlignToReefWithAprilTagCommand.java | 2 +- .../drive/commands/DriveToPose.java | 78 ------------------- 3 files changed, 7 insertions(+), 88 deletions(-) delete mode 100644 src/main/java/competition/subsystems/drive/commands/DriveToPose.java diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index b3580cfa..4e534a8a 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -27,7 +27,6 @@ import competition.subsystems.drive.commands.CalibrateDriveCommand; import competition.subsystems.drive.commands.DebugSwerveModuleCommand; import competition.subsystems.drive.commands.DriveToNearestReefFaceWithPID; -import competition.subsystems.drive.commands.DriveToPose; import competition.subsystems.drive.commands.SwerveDriveWithJoysticksCommand; import competition.subsystems.drive.logic.AlignCameraToAprilTagCalculator; import competition.subsystems.elevator.ElevatorSubsystem; @@ -80,8 +79,7 @@ public void setupDriverCommands( driveToClosestStationCommandGroupFactory, CoprocessorCommunicationSubsystem coprocessorCommunicationSubsystem, PathDriveToLocationForCoralStationFactory pathDriveToLocationForCoralStationFactory, - AlignCameraToAprilTagCalculator.AlignCameraToAprilTagCalculatorFactory aprilTagCalculatorFactory, - DriveToPose driveToPose) { + AlignCameraToAprilTagCalculator.AlignCameraToAprilTagCalculatorFactory aprilTagCalculatorFactory) { resetHeading.setHeadingToApply(0); operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.Start).onTrue(resetHeading); @@ -115,13 +113,12 @@ public void setupDriverCommands( ); SequentialCommandGroup driveToClosestCoralStation = driveToClosestStationCommandGroupFactory.createDriveOnly(true); -// operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( -// pathDriveToClosestCoralStation, -// driveToClosestCoralStation, -// () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) -// )); + operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(new ConditionalCommand( + pathDriveToClosestCoralStation, + driveToClosestCoralStation, + () -> coprocessorCommunicationSubsystem.isCoralStationPathConfident(pose) + )); - operatorInterface.driverGamepad.getifAvailable(XXboxController.XboxButton.A).whileTrue(driveToPose); // operatorInterface.driverGamepad.getPovIfAvailable(0).onTrue(debugModule); // operatorInterface.driverGamepad.getPovIfAvailable(90).onTrue(changeActiveModule); diff --git a/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java b/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java index 0fb2bf60..d98a2fa4 100644 --- a/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java +++ b/src/main/java/competition/subsystems/drive/commands/AlignToReefWithAprilTagCommand.java @@ -75,7 +75,7 @@ public void initialize() { } else { super.setConfigurations( cameraToUse, - aprilTagVisionSubsystem.getTargetAprilTagID(pose.getClosestReefFacePose()), + aprilTagVisionSubsystem.getTargetAprilTagID(pose.getReefFaceFromAngle()), isCameraBackwards, offsetInInches, startingActivity, diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToPose.java b/src/main/java/competition/subsystems/drive/commands/DriveToPose.java deleted file mode 100644 index 74e0906f..00000000 --- a/src/main/java/competition/subsystems/drive/commands/DriveToPose.java +++ /dev/null @@ -1,78 +0,0 @@ -package competition.subsystems.drive.commands; - -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.pose.Landmarks; -import competition.subsystems.pose.PoseSubsystem; -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.util.Units; -import xbot.common.command.BaseCommand; -import xbot.common.math.XYPair; -import xbot.common.subsystems.drive.control_logic.HeadingModule; - -import javax.inject.Inject; - -public class DriveToPose extends BaseCommand { - DriveSubsystem drive; - PoseSubsystem pose; - HeadingModule headingModule; - - @Inject - public DriveToPose(DriveSubsystem drive, PoseSubsystem pose, HeadingModule.HeadingModuleFactory headingModuleFactory) { - this.drive = drive; - this.pose = pose; - this.headingModule = headingModuleFactory.create(drive.getRotateToHeadingPid());; - this.addRequirements(drive); - } - - @Override - public void initialize() { - log.info("Initializing"); - } - - @Override - public void execute() { - var targetPose = PoseSubsystem.convertBlueToRedIfNeeded(pose.getClosestReefFacePose()); - targetPose = PoseSubsystem.convertBlueToRedIfNeeded(Landmarks.BlueCloseRightAlgae); - var currentPose = pose.getCurrentPose2d(); - - var goal = getDriveTarget(currentPose, targetPose); - - double rotationIntent = headingModule.calculateHeadingPower(goal.getRotation()); - - double dx = drive.getPositionalPid().calculate(goal.getX(), currentPose.getX()); - double dy = drive.getPositionalPid().calculate(goal.getY(), currentPose.getY()); - XYPair driveIntent = new XYPair(dx, dy); - - - drive.fieldOrientedDrive(driveIntent, rotationIntent, pose.getCurrentHeading().getDegrees(), new XYPair()); - aKitLog.record("Goal Pose", goal); - } - - public Pose2d getDriveTarget(Pose2d currentPose, Pose2d targetPose) { - - // Final line up - var reefFaceLength = Units.inchesToMeters(36.792600); - var maxDistanceReefLineUp = 1.5; - var offset = currentPose.relativeTo(targetPose); - double yDistance = Math.abs(offset.getY()); - double xDistance = Math.abs(offset.getX()); - double shiftXT = - MathUtil.clamp( - (yDistance / (reefFaceLength * 2)) + ((xDistance - 0.3) / (reefFaceLength * 3)), - 0.0, - 1.0); - double shiftYT = MathUtil.clamp(offset.getX() / reefFaceLength, 0.0, 1.0); - return targetPose.transformBy( - new Transform2d(-shiftXT * maxDistanceReefLineUp, - Math.copySign(shiftYT * maxDistanceReefLineUp * 1.2, offset.getY()), new Rotation2d(0))); - } - - @Override - public void end(boolean isInterrupted) { - super.end(isInterrupted); - drive.stop(); - } -} \ No newline at end of file