Merge branch 'main' of https://github.com/Team6083/2024Overlooking

Team6083 · Aug 10, 2024 · 5a287d1 · 5a287d1
2 parents 8ae119e + ea9ca90
commit 5a287d1
Show file tree

Hide file tree

Showing 2 changed files with 36 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -188,11 +188,7 @@ enum ShooterRotMode {
         mainController.back().onTrue(drivebase.gyroResetCmd());
         controlPanel.button(7).onTrue(drivebase.gyroResetCmd());
     }
-    public Command followPathCommand(String cmdNameString) {
-        // TODO Auto-generated method stub
-        PathPlannerPath path = PathPlannerPath.fromPathFile(cmdNameString);
-       return AutoBuilder.followPath(path);
-    }
+
 
 
     public Command getAutonomousCommand() {

diff --git a/src/main/java/frc/robot/subsystems/drive/Drivebase.java b/src/main/java/frc/robot/subsystems/drive/Drivebase.java
@@ -6,6 +6,7 @@
 
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.commands.FollowPathHolonomic;
 import com.pathplanner.lib.path.PathPlannerPath;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 import com.pathplanner.lib.util.PIDConstants;
@@ -287,6 +288,40 @@ public void putDashboard() {
     SmartDashboard.putNumber("poseY", getPose2d().getY());
     SmartDashboard.putNumber("poseRotationDegree", getPose2d().getRotation().getDegrees());
   }
+
+  // public Command followPathCommand(String cmdNameString) {
+    // TODO Auto-generated method stub
+  //   PathPlannerPath path = PathPlannerPath.fromPathFile(cmdNameString);
+  //  return AutoBuilder.followPath(path);
+
+   public Command followPathCommand(String pathName) {
+    PathPlannerPath path = PathPlannerPath.fromPathFile(pathName);
+
+    return new FollowPathHolonomic(
+            path,
+            this::getPose2d, 
+            this::getRobotRelativeSpeeds, 
+            this::driveRobotRelative, 
+            new HolonomicPathFollowerConfig( 
+                    new PIDConstants(5.0, 0.0, 0.0), 
+                    new PIDConstants(5.0, 0.0, 0.0), 
+                    3.0, 
+                    0.4, 
+                    new ReplanningConfig()
+            ),
+            () -> {
+
+
+              var alliance = DriverStation.getAlliance();
+              if (alliance.isPresent()) {
+                return alliance.get() == DriverStation.Alliance.Red;
+              }
+              return false;
+            },
+            this 
+    );
+}
+