rotation function for pathplanner

HOC-Team-3534 · Feb 16, 2024 · 9a81666 · 9a81666
1 parent 870db99
commit 9a81666
Show file tree

Hide file tree

Showing 8 changed files with 1,005 additions and 30 deletions.
diff --git a/3534CTRESwerve.json b/3534CTRESwerve.json
@@ -1,7 +1,7 @@
 {
   "fileName": "3534CTRESwerve.json",
   "name": "3534CTRESwerve",
-  "version": "2024.1.2",
+  "version": "2024.2.1",
   "frcYear": 2024,
   "uuid": "1fcedb64-1708-4258-9f56-fc21dc0d7c78",
   "mavenUrls": [
@@ -12,7 +12,7 @@
     {
       "groupId": "com.github.HOC-Team-3534",
       "artifactId": "3534CTRESwerve",
-      "version": "2024.1.2"
+      "version": "2024.2.1"
     }
   ],
   "jniDependencies": [],

diff --git a/src/main/java/swerve/CommandSwerveDrivetrain.java b/src/main/java/swerve/CommandSwerveDrivetrain.java
@@ -2,6 +2,7 @@
 
 import java.util.LinkedList;
 import java.util.List;
+import java.util.function.Function;
 import java.util.function.Supplier;
 
 import org.ejml.simple.SimpleMatrix;
@@ -14,10 +15,12 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerTrajectory;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 
 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.SwerveModuleState;
 import edu.wpi.first.wpilibj.Notifier;
 import edu.wpi.first.wpilibj.RobotController;
@@ -72,6 +75,41 @@ public Command pathfindToPose(Pose2d endPose, PathConstraints pathConstraints, d
         return pathplanner.pathfindToPose(endPose, pathConstraints, endVelocity);
     }
 
+    public Command pathfindToPose(Translation2d endPosition, PathConstraints pathConstraints, double endVelocity) {
+        return pathplanner.pathfindToPose(endPosition, this::faceAnySideOfRobotInDirectionOfTravel, pathConstraints,
+                endVelocity);
+    }
+
+    public Rotation2d faceAnySideOfRobotInDirectionOfTravel(PathPlannerTrajectory.State state) {
+        var currentHolonomicRotation = getState().Pose.getRotation();
+        var travelDirection = state.heading;
+
+        var diff = travelDirection.minus(currentHolonomicRotation);
+
+        var diffDegrees = diff.getDegrees() % 360; // -360 to 360
+        while (diffDegrees < -45) {
+            diffDegrees += 90;
+        }
+        while (diffDegrees > 45) {
+            diffDegrees -= 90;
+        }
+        // diffDegrees between -45 to 45;
+
+        return Rotation2d.fromDegrees(currentHolonomicRotation.getDegrees() % 360 + diffDegrees);
+    }
+
+    public Rotation2d faceFrontTowardsRobotDirectionOfTravel(PathPlannerTrajectory.State state) {
+        var travelDirection = state.heading;
+
+        return Rotation2d.fromDegrees(travelDirection.getDegrees() % 360);
+    }
+
+    public Command pathfindToPose(Translation2d endPosition,
+            Function<PathPlannerTrajectory.State, Rotation2d> rotationFunction, PathConstraints pathConstraints,
+            double endVelocity) {
+        return pathplanner.pathfindToPose(endPosition, rotationFunction, pathConstraints, endVelocity);
+    }
+
     public Command followPath(PathPlannerPath path) {
         return pathplanner.followPath(path);
     }

diff --git a/src/main/java/swerve/path/IPathPlanner.java b/src/main/java/swerve/path/IPathPlanner.java
@@ -2,17 +2,21 @@
 
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
+import java.util.function.Function;
 import java.util.function.Supplier;
 
-import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.path.PathConstraints;
 import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPlannerTrajectory;
 import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
 
 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;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import swerve.pathplanner.custom.AutoBuilder;
 
 public interface IPathPlanner {
     default void configureHolonomic(Supplier<Pose2d> poseSupplier, Consumer<Pose2d> resetPose,
@@ -26,6 +30,12 @@ default Command pathfindToPose(Pose2d pose, PathConstraints constraints, double
         return AutoBuilder.pathfindToPose(pose, constraints, goalEndVelocity);
     }
 
+    default Command pathfindToPose(Translation2d position,
+            Function<PathPlannerTrajectory.State, Rotation2d> rotationFunction, PathConstraints constraints,
+            double goalEndVelocity) {
+        return AutoBuilder.pathfindToPose(position, rotationFunction, constraints, goalEndVelocity);
+    }
+
     default Command followPath(PathPlannerPath path) {
         return AutoBuilder.followPath(path);
     }