Update PathPlannerAutos

Add PathGroup Initialization Co-Authored-By: CasualCasuals <[email protected]> Co-Authored-By: Zachary Miller <[email protected]>
nerdherd · Sep 1, 2023 · 433eca8 · 433eca8
1 parent 836f684
commit 433eca8
Show file tree

Hide file tree

Showing 4 changed files with 43 additions and 41 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -257,10 +257,12 @@ private void initAutoChoosers() {
     };
 
     PathPlannerAutos.init(arm, elevator, motorClaw, swerveDrive);
+
 
 
     for (String path : paths) {
       PathPlannerAutos.initPath(path);
+      PathPlannerAutos.initPathGroup(path);
     }
     ShuffleboardTab autosTab = Shuffleboard.getTab("Autos");
 
@@ -275,7 +277,7 @@ private void initAutoChoosers() {
     autoChooser.addOption("Path Planner Test3", () -> PathPlannerAutos.pathplannerAuto("Test Line", swerveDrive));
     autoChooser.addOption("Path Planner TestSquare3", () -> PathPlannerAutos.pathplannerAuto("TestSquare3", swerveDrive));
     autoChooser.addOption("Path Planner TestSquare4", () -> PathPlannerAutos.pathplannerAuto("TestSquare4", swerveDrive));
-    autoChooser.addOption("TestSquare 8/28", () -> new SquareTest(PathPlannerAutos.autoBuilder, false));
+    autoChooser.addOption("TestSquare 8/28", () -> new SquareTest(PathPlannerAutos.autoBuilder));
 
     // TODO: Flip y-values in all old autos (pre-August 2023)
 

diff --git a/src/main/java/frc/robot/commands/PathConstraints.java b/src/main/java/frc/robot/commands/PathConstraints.java
diff --git a/src/main/java/frc/robot/commands/PathPlannerAutos.java b/src/main/java/frc/robot/commands/PathPlannerAutos.java
@@ -1,29 +1,19 @@
 package frc.robot.commands;
 
-import java.io.Console;
 import java.util.HashMap;
+import java.util.List;
 
 import com.pathplanner.lib.PathPlanner;
 import com.pathplanner.lib.PathPlannerTrajectory;
-import com.pathplanner.lib.auto.PIDConstants;
 import com.pathplanner.lib.auto.SwerveAutoBuilder;
-import com.pathplanner.lib.commands.FollowPathWithEvents;
-import com.pathplanner.lib.commands.PPSwerveControllerCommand;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.math.controller.PIDController;
-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.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import frc.robot.Constants.ArmConstants;
 import frc.robot.Constants.ElevatorConstants;
-import frc.robot.Constants.ModuleConstants;
-import frc.robot.Constants.SwerveDriveConstants;
+import frc.robot.Constants.PathPlannerConstants;
 import frc.robot.subsystems.Arm;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.claw.MotorClaw;
@@ -33,6 +23,7 @@
 
 public class PathPlannerAutos {
     private static HashMap<String, PathPlannerTrajectory> cachedPaths = new HashMap<>();
+    private static HashMap<String, List<PathPlannerTrajectory>> cachedPathGroups = new HashMap<>();
     private static HashMap<String, Command> events = new HashMap<>();
 
     public static SwerveAutoBuilder autoBuilder;
@@ -54,12 +45,43 @@ public static void initPath(String pathName) {
         cachedPaths.put(pathName, path);
     }
 
+    public static void initPathGroup(String pathName) {
+        if (cachedPaths.containsKey(pathName)) {
+            DriverStation.reportWarning(String.format("Path '%s' has been loaded more than once.", pathName), true);
+        }
+
+        List<PathPlannerTrajectory> path = PathPlanner.loadPathGroup(pathName, kPPPathConstraints);
+
+        if (path == null || path.size() == 0) {
+            DriverStation.reportWarning(String.format("Path '%s' could not be loaded!", pathName), true);
+        }
+        cachedPathGroups.put(pathName, path);
+    }
+
+    public static List<PathPlannerTrajectory> getPathGroup(String pathNameString) {
+        if (!cachedPathGroups.containsKey(pathNameString)) {
+            DriverStation.reportWarning(String.format("Path '%s' was not pre-loaded into memory, which may cause lag during the Autonomous Period.", pathNameString), true);
+            initPathGroup(pathNameString);
+        }
+        return cachedPathGroups.get(pathNameString);
+
+    }
+
+    public static PathPlannerTrajectory getPath(String pathNameString) {
+        if (!cachedPaths.containsKey(pathNameString)) {
+            DriverStation.reportWarning(String.format("Path '%s' was not pre-loaded into memory, which may cause lag during the Autonomous Period.", pathNameString), true);
+            initPath(pathNameString);
+        }
+        return cachedPaths.get(pathNameString);
+
+    }
+
     public static void init(Arm arm, Elevator elevator, MotorClaw claw, SwerveDrivetrain swerveDrive) {
         autoBuilder = new SwerveAutoBuilder(
             swerveDrive::getPose, 
             swerveDrive::resetOdometry, 
-            new PIDConstants(ModuleConstants.kPDrive, ModuleConstants.kIDrive, ModuleConstants.kDDrive), 
-            new PIDConstants(ModuleConstants.kPTurning, ModuleConstants.kITurning, ModuleConstants.kDTurning), 
+            PathPlannerConstants.kPPTranslationPIDConstants,
+            PathPlannerConstants.kPPRotationPIDConstants,
             swerveDrive::setChassisSpeeds, 
             events,
             swerveDrive);
@@ -138,17 +160,6 @@ public static CommandBase pathplannerAuto(String pathName, SwerveDrivetrain swer
 
         PathPlannerTrajectory path = cachedPaths.get(pathName);
 
-        // Potential issue: RotationConstants doesn't wrap around
-        SwerveAutoBuilder autoBuilder = new SwerveAutoBuilder(
-            swerveDrive::getPose, 
-            swerveDrive::resetOdometry,
-            SwerveDriveConstants.kDriveKinematics,
-            kPPTranslationPIDConstants,
-            kPPRotationPIDConstants,
-            swerveDrive::setModuleStates,
-            events,
-            kUseAllianceColor,
-            swerveDrive);
 
         // Note: The reason why the commands are manually constructed instead of
         // using SwerveAutoBuilder is because SwerveAutoBuilder doesn't

diff --git a/src/main/java/frc/robot/commands/SquareTest.java b/src/main/java/frc/robot/commands/SquareTest.java
@@ -9,18 +9,12 @@
 
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants.SwerveAutoConstants;
 
-public class SquareTest extends SequentialCommandGroup {
-    public SquareTest(SwerveAutoBuilder autoBuilder, boolean isBlue) {
-        List<PathPlannerTrajectory> pathGroup =
-        PathPlanner.loadPathGroup(
-            "TestSquare4",
-            new PathConstraints(SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared),
-            new PathConstraints(SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared),
-            new PathConstraints(SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared),
-            new PathConstraints(SwerveAutoConstants.kMaxSpeedMetersPerSecond, SwerveAutoConstants.kMaxAccelerationMetersPerSecondSquared));
 
+public class SquareTest extends SequentialCommandGroup {
+    public SquareTest(SwerveAutoBuilder autoBuilder) {
+        List<PathPlannerTrajectory> pathGroup = PathPlannerAutos.getPathGroup("TestSquare4");
+
         addCommands(
             Commands.sequence(
                 autoBuilder.resetPose(pathGroup.get(0)),