coord swaps rotation flip

hammerhead226 · Aug 15, 2024 · 0a0ebc9 · 0a0ebc9
1 parent b2176b8
commit 0a0ebc9
Show file tree

Hide file tree

Showing 6 changed files with 50 additions and 91 deletions.
diff --git a/src/main/deploy/pathplanner/paths/New New New New New Path.path b/src/main/deploy/pathplanner/paths/New New New New New Path.path
@@ -3,25 +3,41 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 1.8292964403332503,
+        "y": 8.092188053135022
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 7.0
+        "x": 1.8477029345453428,
+        "y": 8.11979779445316
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 8.317099886696207,
-        "y": 7.424670370025476
+        "x": 1.4598536820392034,
+        "y": 0.8466987337562125
       },
       "prevControl": {
-        "x": 7.317099886696207,
-        "y": 7.424670370025476
+        "x": 0.4598536820392034,
+        "y": 0.8466987337562125
+      },
+      "nextControl": {
+        "x": 2.4598536820392036,
+        "y": 0.8466987337562125
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 15.108269140305099,
+        "y": 4.086241715084328
+      },
+      "prevControl": {
+        "x": 9.041752330141433,
+        "y": 2.112145210837508
       },
       "nextControl": null,
       "isLocked": false,
@@ -39,13 +55,13 @@
   },
   "goalEndState": {
     "velocity": 0,
-    "rotation": 5.909363189683607,
+    "rotation": -133.60281897270377,
     "rotateFast": false
   },
   "reversed": false,
   "folder": null,
   "previewStartingState": {
-    "rotation": 89.3309723976918,
+    "rotation": 43.31531568210366,
     "velocity": 0
   },
   "useDefaultConstraints": true

diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -3,14 +3,14 @@
 /** Automatically generated file containing build version information. */
 public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
-  public static final String MAVEN_NAME = "2024 Robot Code";
+  public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 317;
-  public static final String GIT_SHA = "b87e886bb4404f32a660af7d73877f5015d1e1f5";
-  public static final String GIT_DATE = "2024-08-14 00:51:38 EDT";
+  public static final int GIT_REVISION = 318;
+  public static final String GIT_SHA = "b2176b8c5ef241eedb55dea925bbab869219b7ca";
+  public static final String GIT_DATE = "2024-08-14 20:36:51 EDT";
   public static final String GIT_BRANCH = "note-filtering";
-  public static final String BUILD_DATE = "2024-08-14 20:34:11 EDT";
-  public static final long BUILD_UNIX_TIME = 1723682051564L;
+  public static final String BUILD_DATE = "2024-08-14 21:54:44 EDT";
+  public static final long BUILD_UNIX_TIME = 1723686884925L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}

diff --git a/src/main/java/frc/robot/commands/AlignToNoteAuto.java b/src/main/java/frc/robot/commands/AlignToNoteAuto.java
@@ -77,7 +77,7 @@ public void initialize() {
     pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
     targetNoteLocation = noteLocations.get(drive.getTargetNote());
     useGeneratedPathCommand =
-        drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 2.5
+        drive.getCachedNoteLocation().getDistance(targetNoteLocation) < 1.25
             && drive.getCachedNoteLocation() != null;
     Logger.recordOutput(
         "cached note distance ", drive.getCachedNoteLocation().getDistance(targetNoteLocation));
@@ -89,17 +89,11 @@ public void initialize() {
 
       generatedPathCommand.initialize();
     } else {
-      // targetNoteRotation =
-      //     new Rotation2d(
-      //         targetNoteLocation.getX() - drive.getPose().getX(),
-      //         targetNoteLocation.getY() - drive.getPose().getY());
-      // targetNotePathCommand =
-      //     drive.generateTrajectory(
-      //         new Pose2d(targetNoteLocation, targetNoteRotation), 3, 2.45, 100, 180, 0.5);
-      targetNotePathCommand =
-          AutoBuilder.followPath(drive.generatePathToNoteBlind(targetNoteLocation));
-
-      targetNotePathCommand.initialize();
+    targetNotePathCommand =
+        AutoBuilder.followPath(
+            drive.generateTrajectory(targetNoteLocation, 3, 2.45, 100, 180, 0.5));
+
+    targetNotePathCommand.initialize();
     }
   }
 
@@ -111,7 +105,7 @@ public void execute() {
     if (useGeneratedPathCommand) {
       generatedPathCommand.execute();
     } else {
-      targetNotePathCommand.execute();
+    targetNotePathCommand.execute();
     }
 
     Logger.recordOutput("path is finished", finished);

diff --git a/src/main/java/frc/robot/commands/DriveToChain.java b/src/main/java/frc/robot/commands/DriveToChain.java
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -53,6 +53,7 @@
 import frc.robot.Constants.LED_STATE;
 import frc.robot.Constants.NOTE_POSITIONS;
 import frc.robot.subsystems.led.LED;
+import frc.robot.util.AllianceFlipUtil;
 import frc.robot.util.FieldConstants;
 import frc.robot.util.LimelightHelpers;
 import frc.robot.util.LimelightHelpers.PoseEstimate;
@@ -697,17 +698,19 @@ public Optional<Rotation2d> turnToSpeakerAngle() {
     return Optional.empty();
   }
 
-  public Command generateTrajectory(
-      Pose2d target,
+  public PathPlannerPath generateTrajectory(
+      Translation2d target,
       double maxVelMetersPerSec,
       double maxAccelMetersPerSecSquared,
       double maxAngVelDegPerSec,
       double maxAngAccelDegPerSecSquared,
       double endVelMetersPerSec) {
+    Rotation2d targetRotation =
+        new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY());
     List<Translation2d> points =
         PathPlannerPath.bezierFromPoses(
-            new Pose2d(getPose().getY(), getPose().getX(), getPose().getRotation()),
-            new Pose2d(target.getY(), target.getX(), target.getRotation()));
+            new Pose2d(getPose().getX(), getPose().getY(), getPose().getRotation()),
+            new Pose2d(target.getX(), target.getY(), AllianceFlipUtil.apply(targetRotation)));
     PathPlannerPath path =
         new PathPlannerPath(
             points,
@@ -716,9 +719,9 @@ public Command generateTrajectory(
                 maxAccelMetersPerSecSquared,
                 Units.degreesToRadians(maxAngVelDegPerSec),
                 Units.degreesToRadians(maxAngAccelDegPerSecSquared)),
-            new GoalEndState(endVelMetersPerSec, target.getRotation(), true));
+            new GoalEndState(endVelMetersPerSec, AllianceFlipUtil.apply(targetRotation), true));
 
-    return AutoBuilder.followPath(path);
+    return path;
   }
 
   public void runPath(PathPlannerPath path) {

diff --git a/src/main/java/frc/robot/util/FieldConstants.java b/src/main/java/frc/robot/util/FieldConstants.java
@@ -49,13 +49,13 @@ public static final class StagingLocations {
     static {
       for (int i = 0; i < centerlineTranslations.length; i++) {
         centerlineTranslations[i] =
-            new Translation2d(centerlineFirstY + (i * centerlineSeparationY), centerlineX);
+            new Translation2d(centerlineX, centerlineFirstY + (i * centerlineSeparationY));
       }
     }
 
     static {
       for (int i = 0; i < spikeTranslations.length; i++) {
-        spikeTranslations[i] = new Translation2d(spikeFirstY + (i * spikeSeparationY), spikeX);
+        spikeTranslations[i] = new Translation2d(spikeX, spikeFirstY + (i * spikeSeparationY));
       }
     }
   }