made x angles to speaker using swerve drive

Team334 · Jan 26, 2024 · 3a91fdd · 3a91fdd
1 parent 288efc6
commit 3a91fdd
Show file tree

Hide file tree

Showing 3 changed files with 26 additions and 5 deletions.
diff --git a/src/main/deploy/pathplanner/paths/amp path.path b/src/main/deploy/pathplanner/paths/amp path.path
@@ -58,4 +58,4 @@
   "folder": null,
   "previewStartingState": null,
   "useDefaultConstraints": false
-}
+}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -6,6 +6,7 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

diff --git a/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveDriveSubsystem.java
@@ -7,6 +7,8 @@
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.ReplanningConfig;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -395,6 +397,28 @@ public Rotation2d getHeadingRaw() {
 
   /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */
   public double[] anglesToSpeaker() {
+    int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
+    Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();
+
+    Translation2d tagTranslation = new Translation2d(tagPose.getX(), tagPose.getY());
+    Translation2d botTranslation = _pose.getTranslation();
+
+    double desiredHeading = MathUtil.inputModulus(
+      tagTranslation.minus(botTranslation).getAngle().getDegrees(),
+      -180,
+      180
+    );
+
+    double xAngleToSpeaker = desiredHeading - _pose.getRotation().getDegrees();
+    double yAngleToSpeaker = 0;
+
+    double[] angles = {
+      xAngleToSpeaker,
+      yAngleToSpeaker
+    };
+
+    return angles;
+
     // int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
     // Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();
 
@@ -407,10 +431,6 @@ public double[] anglesToSpeaker() {
 
     // SmartDashboard.putNumber("MOVE ANGLE", moveAngle.getDegrees());
 
-    double[] a = {1, 2};
-
-    return a;
-
     // double xDifference = tagPose.getX() - _pose.getX();
     // double yDifference = tagPose.getY() - _pose.getY();