From 125612790b38fc9402087ec0c5d4e541d51319c6 Mon Sep 17 00:00:00 2001 From: T Grinch <10247070+thenetworkgrinch@users.noreply.github.com> Date: Thu, 21 Mar 2024 15:10:15 -0500 Subject: [PATCH] Fixed aiming towards the speaker. Signed-off-by: thenetworkgrinch --- simgui.json | 3 + src/main/java/frc/robot/RobotContainer.java | 1 + .../swervedrive/SwerveSubsystem.java | 57 +++++++++++-------- 3 files changed, 37 insertions(+), 24 deletions(-) diff --git a/simgui.json b/simgui.json index 21efb849..f3c167a6 100644 --- a/simgui.json +++ b/simgui.json @@ -99,6 +99,9 @@ }, "NetworkTables": { "transitory": { + "FMSInfo": { + "open": true + }, "SmartDashboard": { "Field": { "open": true diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 55bb75ae..34dceccd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -101,6 +101,7 @@ private void configureBindings() Commands.deferredProxy(() -> drivebase.driveToPose( new Pose2d(new Translation2d(4, 4), Rotation2d.fromDegrees(0))) )); + driverXbox.y().whileTrue(drivebase.aimAtSpeaker(2)); // driverXbox.x().whileTrue(Commands.runOnce(drivebase::lock, drivebase).repeatedly()); } diff --git a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java index 0087541f..ca821966 100644 --- a/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swervedrive/SwerveSubsystem.java @@ -12,7 +12,6 @@ import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -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; @@ -29,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config; import frc.robot.Constants.AutonConstants; import java.io.File; -import java.util.Optional; import java.util.function.DoubleSupplier; import org.photonvision.PhotonCamera; import org.photonvision.targeting.PhotonPipelineResult; @@ -88,7 +86,7 @@ public SwerveSubsystem(File directory) throw new RuntimeException(e); } swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. - swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. + swerveDrive.setCosineCompensator(false);//!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); } @@ -148,9 +146,10 @@ public void setupPathPlanner() */ public double getDistanceToSpeaker() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + allianceAprilTag = 7; // Taken from PhotonUtils.getDistanceToPose - Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose( - DriverStation.getAlliance().get() == Alliance.Red ? 4 : 7).get(); + Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); return getPose().getTranslation().getDistance(speakerAprilTagPose.toPose2d().getTranslation()); } @@ -161,26 +160,33 @@ public double getDistanceToSpeaker() */ public Rotation2d getSpeakerYaw() { + int allianceAprilTag = DriverStation.getAlliance().get() == Alliance.Blue ? 7 : 4; + allianceAprilTag = 7; + // Taken from PhotonUtils.getYawToPose() - Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose( - DriverStation.getAlliance().get() == Alliance.Red ? 4 : 7).get(); - Translation2d relativeTrl = speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); - return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()); + Pose3d speakerAprilTagPose = aprilTagFieldLayout.getTagPose(allianceAprilTag).get(); + Translation2d relativeTrl = speakerAprilTagPose.toPose2d().relativeTo(getPose()).getTranslation(); + return new Rotation2d(relativeTrl.getX(), relativeTrl.getY()).plus(swerveDrive.getOdometryHeading()); } /** * Aim the robot at the speaker. + * * @param tolerance Tolerance in degrees. * @return Command to turn the robot to the speaker. */ public Command aimAtSpeaker(double tolerance) { + SwerveController controller = swerveDrive.getSwerveController(); return run( () -> { - drive(getTargetSpeeds(0, - 0, - getSpeakerYaw())); - }).until(() -> getSpeakerYaw().getDegrees() < tolerance); + drive(ChassisSpeeds.fromFieldRelativeSpeeds(0, + 0, + controller.headingCalculate(getHeading().getRadians(), + getSpeakerYaw().getRadians()), + getHeading()) + ); + }).until(() -> getSpeakerYaw().minus(getHeading()).getDegrees() < tolerance); } /** @@ -445,10 +451,11 @@ public void zeroGyro() } /** - * Checks if the alliance is red, defaults to false if alliance isn't available. - * @return true if the red alliance, false if blue. Defaults to false if none is available. - */ - private boolean isRedAlliance() + * Checks if the alliance is red, defaults to false if alliance isn't available. + * + * @return true if the red alliance, false if blue. Defaults to false if none is available. + */ + private boolean isRedAlliance() { var alliance = DriverStation.getAlliance(); return alliance.isPresent() ? alliance.get() == DriverStation.Alliance.Red : false; @@ -456,20 +463,22 @@ private boolean isRedAlliance() /** * This will zero (calibrate) the robot to assume the current position is facing forward - * + *

* If red alliance rotate the robot 180 after the drviebase zero command */ - public void zeroGyroWithAlliance() + public void zeroGyroWithAlliance() { - if (isRedAlliance()) { + if (isRedAlliance()) + { zeroGyro(); //Set the pose 180 degrees resetOdometry(new Pose2d(getPose().getTranslation(), Rotation2d.fromDegrees(180))); - } else { - zeroGyro(); + } else + { + zeroGyro(); } - } - + } + /** * Sets the drive motors to brake/coast mode. *