first good path follow

redshiftrobotics · Oct 18, 2024 · 9d598f3 · 9d598f3
1 parent 47d9e12
commit 9d598f3
Show file tree

Hide file tree

Showing 6 changed files with 70 additions and 99 deletions.
diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto
@@ -5,6 +5,18 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "ArmStow"
+          }
+        },
+        {
+          "type": "wait",
+          "data": {
+            "waitTime": 0.5
+          }
+        },
         {
           "type": "path",
           "data": {
@@ -16,6 +28,12 @@
           "data": {
             "waitTime": 0.5
           }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "2 Test Path"
+          }
         }
       ]
     }

diff --git a/...deploy/pathplanner/paths/Test Path 3.path → ...deploy/pathplanner/paths/2 Test Path.path b/...deploy/pathplanner/paths/Test Path 3.path → ...deploy/pathplanner/paths/2 Test Path.path
@@ -4,24 +4,24 @@
     {
       "anchor": {
         "x": 2.0,
-        "y": 5.0
+        "y": 1.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.0,
-        "y": 6.054664108107561
+        "x": 3.0,
+        "y": 1.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 2.0,
-        "y": 6.0
+        "x": 0.0,
+        "y": 0.0
       },
       "prevControl": {
-        "x": 2.0,
-        "y": 5.107551911484418
+        "x": -1.0,
+        "y": 0.0
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/Test Path 2.path b/src/main/deploy/pathplanner/paths/Test Path 2.path
diff --git a/src/main/deploy/pathplanner/paths/Test Path.path b/src/main/deploy/pathplanner/paths/Test Path.path
@@ -3,25 +3,25 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.0,
-        "y": 7.0
+        "x": 0.0,
+        "y": 0.0
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.0,
-        "y": 7.0
+        "x": 1.0,
+        "y": 0.0
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 4.0,
-        "y": 6.0
+        "x": 2.0,
+        "y": 1.0
       },
       "prevControl": {
-        "x": 3.0,
-        "y": 6.0
+        "x": 1.0,
+        "y": 1.0
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -2,6 +2,7 @@
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -42,7 +43,6 @@
 import frc.robot.utility.OverrideSwitch;
 import frc.robot.utility.logging.Alert;
 import frc.robot.utility.logging.Alert.AlertType;
-import java.util.Optional;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
 /**
@@ -159,6 +159,8 @@ public RobotContainer() {
     // Set up named commands for path planner auto
     // https://pathplanner.dev/pplib-named-commands.html
     NamedCommands.registerCommand("StopWithX", drive.runOnce(drive::stopUsingBrakeArrangement));
+    NamedCommands.registerCommand(
+        "ArmStow", Commands.runOnce(() -> arm.setPosition(ArmConstants.ARM_STOW_2_DEGREES), arm));
 
     // Path planner Autos
     // https://pathplanner.dev/gui-editing-paths-and-autos.html#autos
@@ -222,6 +224,10 @@ private void configureDriverControllerBindings() {
 
       DriverDashboard.getInstance().setSpeedController(speedController);
 
+      SmartDashboard.putData(
+          "Reset Pose",
+          Commands.runOnce(() -> drive.resetPose(new Pose2d())).withName("Reset Pose"));
+
       // Default command
       drive.setDefaultCommand(
           drive
@@ -238,31 +244,31 @@ private void configureDriverControllerBindings() {
                   drive::stop)
               .withName("DefaultDrive"));
 
-      useAngleControlMode
-          .onTrue(
-              Commands.runOnce(
-                      () -> {
-                        headingController.reset();
-                        headingController.setGoal(drive.getPose().getRotation());
-                      })
-                  .withName("PrepareAngleDrive"))
-          .whileTrue(
-              drive
-                  .runEnd(
-                      () -> {
-                        Translation2d translation = input.getTranslationMetersPerSecond();
-                        Optional<Rotation2d> rotation = input.getHeadingDirection();
-                        rotation.ifPresent(headingController::setGoal);
-                        drive.setRobotSpeeds(
-                            speedController.applyTo(
-                                new ChassisSpeeds(
-                                    translation.getX(),
-                                    translation.getY(),
-                                    headingController.calculate())),
-                            useFieldRelative.getAsBoolean());
-                      },
-                      drive::stop)
-                  .withName("RotationAngleDrive"));
+      //   useAngleControlMode
+      //       .onTrue(
+      //           Commands.runOnce(
+      //                   () -> {
+      //                     headingController.reset();
+      //                     headingController.setGoal(drive.getPose().getRotation());
+      //                   })
+      //               .withName("PrepareAngleDrive"))
+      //       .whileTrue(
+      //           drive
+      //               .runEnd(
+      //                   () -> {
+      //                     Translation2d translation = input.getTranslationMetersPerSecond();
+      //                     Optional<Rotation2d> rotation = input.getHeadingDirection();
+      //                     rotation.ifPresent(headingController::setGoal);
+      //                     drive.setRobotSpeeds(
+      //                         speedController.applyTo(
+      //                             new ChassisSpeeds(
+      //                                 translation.getX(),
+      //                                 translation.getY(),
+      //                                 headingController.calculate())),
+      //                         useFieldRelative.getAsBoolean());
+      //                   },
+      //                   drive::stop)
+      //               .withName("RotationAngleDrive"));
 
       boolean includeDiagonalPOV = true;
       for (int pov = 0; pov < 360; pov += includeDiagonalPOV ? 45 : 90) {

diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -133,7 +133,8 @@ public Drive(
         this::getRobotSpeeds,
         this::setRobotSpeeds,
         new HolonomicPathFollowerConfig(
-            new PIDConstants(5),
+            // new PIDConstants(4, 0.01),
+            new PIDConstants(0.1, 0.01),
             new PIDConstants(5),
             DRIVE_CONFIG.maxLinearVelocity(),
             DRIVE_CONFIG.driveBaseRadius(),
@@ -348,12 +349,10 @@ public void setRobotSpeeds(ChassisSpeeds speeds, boolean fieldRelative) {
           setpointGenerator.generateSetpoint(
               currentModuleLimits, currentSetpoint, speeds, Constants.LOOP_PERIOD_SECONDS);
 
-      wheelSpeeds =
-          new SwerveDriveWheelStates(currentSetpoint.moduleStates());
-    }
-    else {
+      wheelSpeeds = new SwerveDriveWheelStates(currentSetpoint.moduleStates());
+    } else {
       speeds = ChassisSpeeds.discretize(speeds, Constants.LOOP_PERIOD_SECONDS);
-      
+
       wheelSpeeds = kinematics.toWheelSpeeds(speeds);
     }