Face fixture wrap fix

.vscode/settings.json

@@ -57,5 +57,13 @@
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
   ],
-  "java.dependency.enableDependencyCheckup": false
+  "java.dependency.enableDependencyCheckup": false,
+  "cSpell.words": [
+    "Deadband",
+    "feedforwards",
+    "Holonomic",
+    "Photonvision",
+    "Pidgey",
+    "teleop"
+  ]
 }

src/main/java/frc/robot/Constants.java

@@ -144,4 +144,8 @@ public static final class VisionConstants {
         public static final AprilTagFieldLayout kTagLayout
                 = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
     }
+
+    public static final class NumericalConstants {
+        public static final double kEpsilon = 1e-6;
+    }
 }

src/main/java/frc/robot/Meters.java

@@ -0,0 +1,5 @@
+package frc.robot;
+
+public class Meters {
+
+}

src/main/java/frc/robot/RobotContainer.java

@@ -6,6 +6,7 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -14,7 +15,6 @@
 import frc.robot.Constants.AutoConstants;
 import frc.robot.Constants.OIConstants;
 import frc.robot.subsystems.DriveSubsystem;
-import static edu.wpi.first.units.Units.*;
 
 public class RobotContainer {
 
@@ -46,7 +46,7 @@ public RobotContainer() {
     }
 
     private void configureBindings() {
-        m_driverController.x().whileTrue(m_drive.moveToAngle(Radians.of(Math.PI)));
+        m_driverController.x().whileTrue(m_drive.enableFacePose(new Pose2d()));
         m_driverController.x().whileFalse(m_drive.disableFacePose());
     }
 

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -36,10 +36,17 @@
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.units.measure.*;
 
 import static edu.wpi.first.units.Units.*;
 
+import java.util.Optional;
+
+import org.photonvision.PhotonCamera;
+
+import frc.robot.Constants;
+
 public class DriveSubsystem extends SubsystemBase {
 
     // Create MAXSwerveModules
@@ -166,11 +173,13 @@ public Command enableFacePose(Pose2d fixture) {
             double xFixtureDist = fixture.getX() - robotPose.getX();
             double yFixtureDist = fixture.getY() - robotPose.getY();
 
-            double angleToFixture = Math.atan2(yFixtureDist, xFixtureDist);
-
-            System.out.println(angleToFixture);
+            double totalDistance = Math.hypot(xFixtureDist, yFixtureDist);
 
-            m_targetAutoAngle = Radians.of(angleToFixture);
+            // Floating point value correction
+            if (Math.abs(totalDistance) < Constants.NumericalConstants.kEpsilon)
+                return;
+
+            m_targetAutoAngle = Radians.of(Math.atan2(yFixtureDist, xFixtureDist));
 
             m_isManualRotate = false;
         });
@@ -265,10 +274,14 @@ public void resetOdometry(Pose2d pose) {
     public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
         // Convert the commanded speeds into the correct units for the drivetrain
 
+        if (!m_isManualRotate)
+            System.out.println("Setpoint: " + getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians) + ", Current: "
+                    + getHeading().in(Radians));
+
         double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeed.magnitude();
         double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeed.magnitude();
         double rotDelivered = (m_isManualRotate) ? rot * DriveConstants.kMaxAngularSpeed.magnitude()
-                : m_pidController.calculate(getHeading().in(Radians), getOptimalAngle(m_targetAutoAngle).in(Radians));
+                : m_pidController.calculate(getHeading().in(Radians), getOptimalAngle(m_targetAutoAngle, getHeading()).in(Radians));
 
         var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
                 fieldRelative
@@ -353,21 +366,23 @@ public Angle getNonContinuousHeading() {
         return Radians.of(getHeading().in(Radians) - rotations);
     }
 
-    private Angle getOptimalAngle(Angle target) {
-        Angle robotHeading = getHeading();
-
+    private Angle getOptimalAngle(Angle target, Angle robotHeading) {
         // Full robot rotations in radians
-        Angle robotRotations = Radians.of(Radians.convertFrom(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)), Rotations));
+        Angle robotRotations = Radians
+                .of(Math.floor(robotHeading.in(Radians) / (2 * Math.PI)) * 2.0 * Math.PI);
 
-        // Both are the same angle, just one is negative and one is positive
-        Angle pTargetAngle = robotRotations.plus(target);
-        Angle nTargetAngle = robotRotations.plus(target.minus(Radians.of(2 * Math.PI)));
+        Angle wrappedRobotAngle = robotHeading.minus(robotRotations);
 
-        // If either angle is less than 180 degrees relative to the robot's current angle, it is the most optimal path
-        if (robotHeading.minus(pTargetAngle).abs(Radians) < Math.PI) {
-            return pTargetAngle;
-        }
+        Angle delta = target.minus(wrappedRobotAngle);
+
+        // Ensuring that the angle is always positive to ensure it is wrapped correctly
+        if (delta.lt(Radians.of(0.0)))
+            delta = delta.plus(Radians.of(2 * Math.PI));
+
+        // Wrapping the delta to make it at most 180 deg
+        if (delta.gt(Radians.of(Math.PI)))
+            delta = delta.minus(Radians.of(2.0 * Math.PI));
 
-        return nTargetAngle;
+        return delta.plus(robotHeading);
     }
 }