add missing paths for automation (#191)

team401 · Apr 18, 2024 · 3990770 · 3990770
1 parent 14f6a9b
commit 3990770
Show file tree

Hide file tree

Showing 7 changed files with 202 additions and 67 deletions.
diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json
@@ -3,7 +3,8 @@
   "robotLength": 0.9525,
   "holonomicMode": true,
   "pathFolders": [
-    "Endgame"
+    "Endgame",
+    "Source"
   ],
   "autoFolders": [],
   "defaultMaxVel": 4.0,

diff --git a/src/main/deploy/pathplanner/paths/rightSource.path b/src/main/deploy/pathplanner/paths/rightSource.path
@@ -0,0 +1,55 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.432455267688313,
+        "y": 1.0401173811606885
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 4.699058199042324,
+        "y": 1.5593743826376547
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.3367740994741248,
+        "y": 1.6330703957704211
+      },
+      "prevControl": {
+        "x": 1.5714611614408416,
+        "y": 2.8244991721464925
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 0.5,
+      "rotationDegrees": -49.42789973843088,
+      "rotateFast": false
+    }
+  ],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 12.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -118.51259584088376,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source",
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/shopSource.path b/src/main/deploy/pathplanner/paths/shopSource.path
@@ -0,0 +1,49 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 6.965564788272434,
+        "y": 6.4355020138611705
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 7.595921477557241,
+        "y": 6.391523640190138
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 8.29957545629377,
+        "y": 6.801988461119779
+      },
+      "prevControl": {
+        "x": 7.874451177473783,
+        "y": 6.582096592764614
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 12.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -150.461217740442,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": "Source",
+  "previewStartingState": null,
+  "useDefaultConstraints": false
+}
diff --git a/src/main/deploy/pathplanner/paths/speaker.path b/src/main/deploy/pathplanner/paths/speaker.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 8.226278166842048,
+        "y": 6.420842555970826
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.24725135164556,
+        "y": 8.179977502812148
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.6588410319677789,
+        "y": 5.541275082550166
+      },
+      "prevControl": {
+        "x": 0.6588410319677789,
+        "y": 5.541275082550166
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 4.0,
+    "maxAcceleration": 12.0,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -177.13759477388828,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": -135.88140399658215,
+    "velocity": 0
+  },
+  "useDefaultConstraints": true
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -52,9 +52,9 @@ public static final class FeatureFlags {
         public static final boolean runVision = true;
         public static final boolean runLocalizer = true;
 
-        public static final boolean runIntake = true;
-        public static final boolean runScoring = true;
-        public static final boolean runEndgame = true;
+        public static final boolean runIntake = false;
+        public static final boolean runScoring = false;
+        public static final boolean runEndgame = false;
         public static final boolean runDrive = true;
 
         public static final boolean enableLEDS = true;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -329,9 +329,13 @@ private void configureBindings() {
                 .onTrue(new InstantCommand(
                     () -> drivetrain.setAlignTarget(AlignTarget.RIGHT)));
 
-            // controller.x()
-            //     .onTrue(new InstantCommand(() -> drivetrain.driveToEndgame()))
-            //     .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
+            controller.x()
+                .onTrue(new InstantCommand(() -> drivetrain.driveToSource()))
+                .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
+
+            controller.y()
+                .onTrue(new InstantCommand(() -> drivetrain.driveToSpeaker()))
+                .onFalse(new InstantCommand(() -> drivetrain.stopDriveToPose()));
         }
 
         if (FeatureFlags.runEndgame) {
@@ -935,7 +939,7 @@ public void disabledPeriodic() {
                 drivetrain.setBrakeMode(true);
             }
         }
-        if (ledSwitch != null) {
+        if (ledSwitch != null && leds != null) {
             leds.setEnabled(!ledSwitch.get());
         }
     }

diff --git a/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/drive/CommandSwerveDrivetrain.java
@@ -16,6 +16,7 @@
 import com.pathplanner.lib.pathfinding.Pathfinding;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
+import com.pathplanner.lib.util.PathPlannerLogging;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -550,6 +551,31 @@ public void driveToPose(Pose2d targetPose) {
         pathfindCommand.schedule();
     }
 
+    public void driveToPath(String pathName) {
+        this.setAlignState(AlignState.MANUAL);
+
+        PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);
+
+        // if(DriverStation.getAlliance() === Alliance.Blue) {
+        //     path.flipPath();
+        // }
+
+        PathConstraints constraints =
+                new PathConstraints(
+                        3.0,
+                        4.0,
+                        Constants.ConversionConstants.kDegreesToRadians * 540,
+                        Constants.ConversionConstants.kDegreesToRadians * 720);
+
+        PathPlannerLogging.setLogTargetPoseCallback(
+                (target) -> {
+                    Logger.recordOutput("targetPose", target);
+                });
+
+        pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
+        pathfindCommand.schedule();
+    }
+
     public void stopDriveToPose() {
         if (pathfindCommand != null) {
             pathfindCommand.cancel();
@@ -562,50 +588,12 @@ public void setPoseTarget(Pose2d pose) {
         targetTightPose = pose;
     }
 
-    public Pose2d getEndgamePose() {
-        // Blue Alliance Poses
-        Pose2d leftClimbPose2d = new Pose2d(4.61, 4.48, Rotation2d.fromDegrees(-60));
-        Pose2d rightClimbPose2d = new Pose2d(4.66, 3.67, Rotation2d.fromDegrees(60));
-        Pose2d farClimbPose2d = new Pose2d(5.38, 4.11, Rotation2d.fromDegrees(180));
-
-        if (DriverStation.getAlliance().isPresent()
-                && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) {
-            // Red Alliance Poses
-            leftClimbPose2d = new Pose2d(11.882, 3.67, Rotation2d.fromDegrees(120));
-            rightClimbPose2d = new Pose2d(11.932, 4.48, Rotation2d.fromDegrees(-120));
-            farClimbPose2d = new Pose2d(11.162, 4.11, Rotation2d.fromDegrees(0));
-        }
-
-        double distanceToTargetLeft =
-                Math.hypot(
-                        getFieldToRobot.get().getX() - leftClimbPose2d.getX(),
-                        getFieldToRobot.get().getY() - leftClimbPose2d.getY());
-        double distanceToTargetRight =
-                Math.hypot(
-                        getFieldToRobot.get().getX() - rightClimbPose2d.getX(),
-                        getFieldToRobot.get().getY() - rightClimbPose2d.getY());
-        double distanceToTargetFar =
-                Math.hypot(
-                        getFieldToRobot.get().getX() - farClimbPose2d.getX(),
-                        getFieldToRobot.get().getY() - farClimbPose2d.getY());
-
-        Pose2d targetPose = new Pose2d(new Translation2d(), Rotation2d.fromDegrees(180));
-
-        if (distanceToTargetLeft < distanceToTargetRight
-                && distanceToTargetLeft < distanceToTargetFar) {
-            targetPose = leftClimbPose2d;
-        } else if (distanceToTargetRight < distanceToTargetLeft
-                && distanceToTargetRight < distanceToTargetFar) {
-            targetPose = rightClimbPose2d;
-        } else {
-            targetPose = farClimbPose2d;
-        }
-
-        return targetPose;
+    public void driveToSpeaker() {
+        driveToPath("speaker");
     }
 
-    public void driveToSpeaker() {
-        driveToPose(new Pose2d(AllianceUtil.getFieldToSpeaker(), new Rotation2d()));
+    public void driveToSource() {
+        driveToPath("shopSource");
     }
 
     public void driveToEndgame() {
@@ -635,29 +623,15 @@ public void driveToEndgame() {
                         getFieldToRobot.get().getX() - farClimbPose2d.getX(),
                         getFieldToRobot.get().getY() - farClimbPose2d.getY());
 
-        PathPlannerPath path = null;
-
         if (distanceToTargetLeft < distanceToTargetRight
                 && distanceToTargetLeft < distanceToTargetFar) {
-            path = PathPlannerPath.fromPathFile("LeftEndgame");
+            driveToPath("leftEndgame");
         } else if (distanceToTargetRight < distanceToTargetLeft
                 && distanceToTargetRight < distanceToTargetFar) {
-            path = PathPlannerPath.fromPathFile("RightEndgame");
+            driveToPath("rightEndgame");
         } else {
-            path = PathPlannerPath.fromPathFile("FarEndgame");
+            driveToPath("farEndgame");
         }
-
-        this.setAlignState(AlignState.MANUAL);
-
-        PathConstraints constraints =
-                new PathConstraints(
-                        3.0,
-                        4.0,
-                        Constants.ConversionConstants.kDegreesToRadians * 540,
-                        Constants.ConversionConstants.kDegreesToRadians * 720);
-
-        pathfindCommand = AutoBuilder.pathfindThenFollowPath(path, constraints, 0.0);
-        pathfindCommand.schedule();
     }
 
     public boolean isAligned() {