Flip driver control on the red alliance (#30)

* fix: use correct version of LocalADStarAK

* feat: flip driver control on red alliance

* fix: check flipping in intialize

---------

Co-authored-by: Zach Rutman <[email protected]>

src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import org.team1540.robot2024.subsystems.drive.Drivetrain;
@@ -19,6 +20,8 @@ public class SwerveDriveCommand extends Command {
     private final SlewRateLimiter yLimiter = new SlewRateLimiter(2);
     private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
 
+    private boolean isFlipped;
+
     public SwerveDriveCommand(Drivetrain drivetrain, CommandXboxController controller) {
         this.drivetrain = drivetrain;
         this.controller = controller;
@@ -30,6 +33,9 @@ public void initialize() {
         xLimiter.reset(0);
         yLimiter.reset(0);
         rotLimiter.reset(0);
+        isFlipped =
+                DriverStation.getAlliance().isPresent()
+                        && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
     }
 
     @Override
@@ -54,7 +60,9 @@ public void execute() {
                         linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(),
                         linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(),
                         omega * drivetrain.getMaxAngularSpeedRadPerSec(),
-                        drivetrain.getRotation()
+                        isFlipped
+                                ? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180))
+                                : drivetrain.getRotation()
                 )
         );
     }

src/main/java/org/team1540/robot2024/util/LocalADStarAK.java

@@ -29,7 +29,7 @@ public class LocalADStarAK implements Pathfinder {
      */
     @Override
     public boolean isNewPathAvailable() {
-        if (Logger.hasReplaySource()) {
+        if (!Logger.hasReplaySource()) {
             io.updateIsNewPathAvailable();
         }
 
@@ -41,13 +41,13 @@ public boolean isNewPathAvailable() {
     /**
      * Get the most recently calculated path
      *
-     * @param constraints  The path constraints to use when creating the path
+     * @param constraints The path constraints to use when creating the path
      * @param goalEndState The goal end state to use when creating the path
      * @return The PathPlannerPath created from the points calculated by the pathfinder
      */
     @Override
     public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
-        if (Logger.hasReplaySource()) {
+        if (!Logger.hasReplaySource()) {
             io.updateCurrentPathPoints(constraints, goalEndState);
         }
 
@@ -64,11 +64,11 @@ public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState
      * Set the start position to pathfind from
      *
      * @param startPosition Start position on the field. If this is within an obstacle it will be
-     *                      moved to the nearest non-obstacle node.
+     *     moved to the nearest non-obstacle node.
      */
     @Override
     public void setStartPosition(Translation2d startPosition) {
-        if (Logger.hasReplaySource()) {
+        if (!Logger.hasReplaySource()) {
             io.adStar.setStartPosition(startPosition);
         }
     }
@@ -77,27 +77,29 @@ public void setStartPosition(Translation2d startPosition) {
      * Set the goal position to pathfind to
      *
      * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
-     *                     to the nearest non-obstacle node.
+     *     to the nearest non-obstacle node.
      */
     @Override
     public void setGoalPosition(Translation2d goalPosition) {
-        if (Logger.hasReplaySource()) {
+        if (!Logger.hasReplaySource()) {
             io.adStar.setGoalPosition(goalPosition);
         }
     }
 
     /**
      * Set the dynamic obstacles that should be avoided while pathfinding.
      *
-     * @param obs             A List of Translation2d pairs representing obstacles. Each Translation2d represents
-     *                        opposite corners of a bounding box.
+     * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+     *     opposite corners of a bounding box.
      * @param currentRobotPos The current position of the robot. This is needed to change the start
-     *                        position of the path to properly avoid obstacles
+     *     position of the path to properly avoid obstacles
      */
     @Override
     public void setDynamicObstacles(
             List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
-        io.adStar.setDynamicObstacles(obs, currentRobotPos);
+        if (!Logger.hasReplaySource()) {
+            io.adStar.setDynamicObstacles(obs, currentRobotPos);
+        }
     }
 
     private static class ADStarIO implements LoggableInputs {
@@ -128,8 +130,7 @@ public void fromLog(LogTable table) {
 
             List<PathPoint> pathPoints = new ArrayList<>();
             for (int i = 0; i < pointsLogged.length; i += 2) {
-                pathPoints.add(
-                        new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
+                pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
             }
 
             currentPathPoints = pathPoints;
@@ -149,4 +150,4 @@ public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState go
             }
         }
     }
-}
+}