From 9590c17095f8efaf6d81ca82b10f1d66884c91cc Mon Sep 17 00:00:00 2001 From: WowMuchDoge <97766806+WowMuchDoge@users.noreply.github.com> Date: Mon, 19 Jan 2026 17:59:24 -0600 Subject: [PATCH] Face fixture wrap fix --- .vscode/settings.json | 10 +++- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/Meters.java | 5 ++ src/main/java/frc/robot/RobotContainer.java | 4 +- .../frc/robot/subsystems/DriveSubsystem.java | 49 ++++++++++++------- 5 files changed, 52 insertions(+), 20 deletions(-) create mode 100644 src/main/java/frc/robot/Meters.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 5e6ede8..fef457d 100644 --- a/.vscode/settings.json +++ b/.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" + ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1cc3a86..d5d42e0 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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; + } } diff --git a/src/main/java/frc/robot/Meters.java b/src/main/java/frc/robot/Meters.java new file mode 100644 index 0000000..2903454 --- /dev/null +++ b/src/main/java/frc/robot/Meters.java @@ -0,0 +1,5 @@ +package frc.robot; + +public class Meters { + +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6796a37..c671577 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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()); } diff --git a/src/main/java/frc/robot/subsystems/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/DriveSubsystem.java index e1c77fc..7d9088e 100644 --- a/src/main/java/frc/robot/subsystems/DriveSubsystem.java +++ b/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); } }