Merge branch 'preDistrictComp_driveToAmp' of https://github.com/Devil…

…Botz2876/Crescendo2024 into preDistrictComp_secondIntakeSensor
DevilBotz2876 · Apr 2, 2024 · 776146f · 776146f
2 parents 189022c + 7cab942
commit 776146f
Show file tree

Hide file tree

Showing 13 changed files with 74 additions and 92 deletions.
diff --git a/src/main/deploy/pathplanner/paths/1 to 7.path b/src/main/deploy/pathplanner/paths/1 to 7.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.6,
+        "x": 2.65,
         "y": 3.9
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.6,
+        "x": 2.6500000000000004,
         "y": 2.9
       },
       "isLocked": true,

diff --git a/src/main/deploy/pathplanner/paths/1 to C.path b/src/main/deploy/pathplanner/paths/1 to C.path
@@ -8,7 +8,7 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.2964466094067264,
+        "x": 2.2964466094067273,
         "y": 4.253553390593277
       },
       "isLocked": true,

diff --git a/src/main/deploy/pathplanner/paths/3 to 5.path b/src/main/deploy/pathplanner/paths/3 to 5.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 7.873688005440527,
-        "y": 5.777446393602466
+        "x": 8.0,
+        "y": 5.75
       },
       "prevControl": {
-        "x": 6.967380218403877,
-        "y": 6.200064655343165
+        "x": 7.09369221296335,
+        "y": 6.172618261740699
       },
       "nextControl": null,
       "isLocked": true,

diff --git a/src/main/deploy/pathplanner/paths/3 to Between 2-3.path b/src/main/deploy/pathplanner/paths/3 to Between 2-3.path
@@ -3,12 +3,12 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.7,
+        "x": 2.65,
         "y": 6.8
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.1767288353915135,
+        "x": 2.1267288353915133,
         "y": 6.669526014370095
       },
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/5 to 3.path b/src/main/deploy/pathplanner/paths/5 to 3.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 7.873688005440527,
-        "y": 5.777446393602466
+        "x": 8.0,
+        "y": 5.75
       },
       "prevControl": null,
       "nextControl": {
-        "x": 6.907762179151458,
-        "y": 6.036265438704987
+        "x": 7.034074173710931,
+        "y": 6.008819045102522
       },
       "isLocked": true,
       "linkedName": "\"5\" Center Amp Mid Note"

diff --git a/src/main/deploy/pathplanner/paths/7 to 1.path b/src/main/deploy/pathplanner/paths/7 to 1.path
@@ -32,11 +32,11 @@
     },
     {
       "anchor": {
-        "x": 2.6,
+        "x": 2.65,
         "y": 3.9
       },
       "prevControl": {
-        "x": 2.6,
+        "x": 2.6500000000000004,
         "y": 2.9
       },
       "nextControl": null,

diff --git a/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path b/src/main/deploy/pathplanner/paths/Between 2-3 to 3.path
@@ -16,11 +16,11 @@
     },
     {
       "anchor": {
-        "x": 2.7,
+        "x": 2.65,
         "y": 6.8
       },
       "prevControl": {
-        "x": 2.3229504000992742,
+        "x": 2.272950400099274,
         "y": 6.5233044496623345
       },
       "nextControl": null,

diff --git a/src/main/deploy/pathplanner/paths/C to 1.path b/src/main/deploy/pathplanner/paths/C to 1.path
@@ -20,7 +20,7 @@
         "y": 3.9
       },
       "prevControl": {
-        "x": 1.65,
+        "x": 1.6500000000000004,
         "y": 3.9
       },
       "nextControl": null,

diff --git a/src/main/deploy/pathplanner/paths/P to 1.path b/src/main/deploy/pathplanner/paths/P to 1.path
@@ -16,15 +16,15 @@
     },
     {
       "anchor": {
-        "x": 10.0,
-        "y": 2.0
+        "x": 2.65,
+        "y": 3.9
       },
       "prevControl": {
-        "x": 9.0,
-        "y": 1.9999999999999998
+        "x": 1.6500000000000004,
+        "y": 3.8999999999999995
       },
       "nextControl": null,
-      "isLocked": true,
+      "isLocked": false,
       "linkedName": "\"1\" Wing Podium Note"
     }
   ],

diff --git a/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path b/src/main/deploy/pathplanner/paths/P to 8 Center Sweep.path
@@ -25,10 +25,10 @@
       },
       "nextControl": {
         "x": 10.0,
-        "y": 3.0
+        "y": 3.0000000000000004
       },
       "isLocked": false,
-      "linkedName": "\"1\" Wing Podium Note"
+      "linkedName": null
     },
     {
       "anchor": {

diff --git a/src/main/java/frc/robot/controls/DebugControls.java b/src/main/java/frc/robot/controls/DebugControls.java
@@ -1,12 +1,9 @@
 package frc.robot.controls;
 
 import com.pathplanner.lib.auto.AutoBuilder;
-import com.pathplanner.lib.path.GoalEndState;
 import com.pathplanner.lib.path.PathConstraints;
-import com.pathplanner.lib.path.PathPlannerPath;
 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.util.Units;
 import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
@@ -175,35 +172,12 @@ public static void setupControls() {
         .withPosition(colIndex, rowIndex++)
         .withSize(2, 1);
 
-    // Create a list of bezier points from poses. Each pose represents one waypoint.
-    // The rotation component of the pose should be the direction of travel. Do not use holonomic
-    // rotation.
-    List<Translation2d> bezierPoints =
-        PathPlannerPath.bezierFromPoses(
-            RobotConfig.drive.getPose(),
-            new Pose2d(1.8, 7.5, Rotation2d.fromDegrees(90)),
-            new Pose2d(1.8, 7.7, Rotation2d.fromDegrees(90)));
-
-    // Create the path using the bezier points created above
-    PathPlannerPath path =
-        new PathPlannerPath(
-            bezierPoints,
-            new PathConstraints(
-                1.0,
-                3.0,
-                2 * Math.PI,
-                4 * Math.PI), // The constraints for this path. If using a differential drivetrain,
-            // the angular constraints have no effect.
-            new GoalEndState(
-                0.0,
-                Rotation2d.fromDegrees(
-                    -90)) // Goal end state. You can set a holonomic rotation here. If using a
-            // differential drivetrain, the rotation will have no effect.
-            );
-
-    Command driveToBlueAmp = AutoBuilder.followPath(path);
-    driveToBlueAmp.setName("Drive To Amp");
-    debugTab.add(driveToBlueAmp).withPosition(colIndex, rowIndex++).withSize(2, 1);
+    Command driveToAmp =
+        AutoBuilder.pathfindToPoseFlipped(
+            new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)),
+            new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI));
+    driveToAmp.setName("Drive To Amp");
+    debugTab.add(driveToAmp).withPosition(colIndex, rowIndex++).withSize(2, 1);
     ;
     /*
         colIndex += 2;

diff --git a/src/main/java/frc/robot/controls/DriverControls.java b/src/main/java/frc/robot/controls/DriverControls.java
@@ -1,6 +1,10 @@
 package frc.robot.controls;
 
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.path.PathConstraints;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID.RumbleType;
@@ -17,6 +21,7 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SelectCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -198,14 +203,11 @@ private static void setupCommonControls(CommandXboxController controller) {
     /*     Y Button = Prepare to Climb
      *     X Button = Climber
      */
-    controller
-        .y()
-        .onTrue(
-            new ParallelCommandGroup(
-                RobotConfig.intake.getTurnOffCommand(),
-                RobotConfig.shooter.getTurnOffCommand(),
-                RobotConfig.arm.getStowCommand(),
-                RobotConfig.climber.getExtendCommand()));
+    controller.y().onTrue(RobotConfig.intake.getTurnOffCommand());
+    controller.y().onTrue(RobotConfig.shooter.getTurnOffCommand());
+    controller.y().onTrue(RobotConfig.arm.getStowCommand());
+    controller.y().onTrue(RobotConfig.climber.getExtendCommand());
+
     controller.x().onTrue(RobotConfig.climber.getRetractCommand());
 
     /* Target Selection Controls */
@@ -288,30 +290,44 @@ public static void setupMainControls(CommandXboxController mainController) {
         .onTrue(
             new EjectPiece(RobotConfig.intake, RobotConfig.arm, RobotConfig.shooter)); // Eject Note
 
+    mainController.rightBumper().onTrue(RobotConfig.intake.getTurnOffCommand());
+
     mainController
         .rightBumper()
         .onTrue(
-            new ParallelCommandGroup(
-                RobotConfig.intake.getTurnOffCommand(),
-                new DriveToYaw(RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget())
-                    .withTimeout(DriveConstants.pidTimeoutInSeconds),
-                new SetShooterVelocity(
-                        RobotConfig.shooter, () -> DevilBotState.getShooterVelocity())
-                    .withTimeout(ShooterConstants.pidTimeoutInSeconds), // turn on shooter
-                /* TODO: Use ArmToPositionTP instead of setting arm angle directly */
-                new InstantCommand(
+            new SelectCommand<>(
+                Map.ofEntries(
+                    Map.entry(
+                        true,
+                        AutoBuilder.pathfindToPoseFlipped(
+                            new Pose2d(1.8, 7.75, Rotation2d.fromDegrees(-90)),
+                            new PathConstraints(4.0, 3.0, 2 * Math.PI, 3 * Math.PI))),
+                    Map.entry(
+                        false,
+                        new DriveToYaw(
+                                RobotConfig.drive, () -> DevilBotState.getVisionRobotYawToTarget())
+                            .withTimeout(DriveConstants.pidTimeoutInSeconds))),
+                () -> DevilBotState.isAmpMode()));
+
+    mainController
+        .rightBumper()
+        .onTrue(
+            new SetShooterVelocity(RobotConfig.shooter, () -> DevilBotState.getShooterVelocity())
+                .withTimeout(ShooterConstants.pidTimeoutInSeconds) // turn on shooter
+            );
+
+    mainController
+        .rightBumper()
+        .onTrue(
+            new InstantCommand(
                     () -> {
-                      if (DevilBotState.isAmpMode()) {
-                        RobotConfig.arm.setAngle(ArmConstants.ampScoreAngleInDegrees);
-                      } else {
-                        Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
-                        if (armAngle.isPresent()) {
-                          RobotConfig.arm.setAngle((armAngle.get()));
-                        }
+                      Optional<Double> armAngle = DevilBotState.getArmAngleToTarget();
+                      if (armAngle.isPresent()) {
+                        RobotConfig.arm.setAngle((armAngle.get()));
                       }
                     },
-                    RobotConfig.arm) // adjust arm angle based on vision's distance from target
-                )); // Aim
+                    RobotConfig.arm)
+                .onlyIf(() -> !DevilBotState.isAmpMode()));
 
     mainController
         .rightTrigger()

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java b/src/main/java/frc/robot/subsystems/drive/DriveSwerveYAGSL.java
@@ -2,7 +2,6 @@
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
-import com.pathplanner.lib.util.PIDConstants;
 import com.pathplanner.lib.util.ReplanningConfig;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.filter.SlewRateLimiter;
@@ -20,7 +19,6 @@
 import org.littletonrobotics.junction.Logger;
 import swervelib.SwerveDrive;
 import swervelib.SwerveDriveTest;
-import swervelib.parser.PIDFConfig;
 import swervelib.parser.SwerveParser;
 import swervelib.telemetry.SwerveDriveTelemetry;
 
@@ -55,9 +53,6 @@ public DriveSwerveYAGSL(String configPath) {
       throw new RuntimeException(e);
     }
 
-    PIDFConfig drivePIDF = swerveDrive.getModules()[0].getDrivePIDF();
-    PIDFConfig anglePIDF = swerveDrive.getModules()[0].getAnglePIDF();
-
     AutoBuilder.configureHolonomic(
         swerveDrive::getPose, // Robot pose supplier
         swerveDrive
@@ -66,10 +61,7 @@ public DriveSwerveYAGSL(String configPath) {
         swerveDrive::getRobotVelocity, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE
         swerveDrive::setChassisSpeeds, // Method that will drive the robot given ROBOT RELATIVE
         // ChassisSpeeds
-        new HolonomicPathFollowerConfig( // HolonomicPathFollowerConfig, this should likely live in
-            // your Constants class
-            new PIDConstants(drivePIDF.p, drivePIDF.i, drivePIDF.d), // Translation PID constants
-            new PIDConstants(anglePIDF.p, anglePIDF.i, anglePIDF.d), // Rotation PID constants
+        new HolonomicPathFollowerConfig(
             swerveDrive.getMaximumVelocity(), // Max module speed, in m/s
             swerveDrive.swerveDriveConfiguration
                 .getDriveBaseRadiusMeters(), // Drive base radius in meters. Distance from robot