Fixed aiming towards the speaker.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

simgui.json

@@ -99,6 +99,9 @@
   },
   "NetworkTables": {
     "transitory": {
+      "FMSInfo": {
+        "open": true
+      },
       "SmartDashboard": {
         "Field": {
           "open": true

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());
   }
 

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,31 +451,34 @@ 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;
   }
 
   /**
    * This will zero (calibrate) the robot to assume the current position is facing forward
-   * 
+   * <p>
    * 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.
    *