From e524577c90b58767635f5f7e68ddfe1b1bd3d9ed Mon Sep 17 00:00:00 2001
From: Alvin Zhang <41vin2h4n9@gmail.com>
Date: Sat, 10 Feb 2024 12:07:34 -0800
Subject: [PATCH] fix: driver flipping needs to be done in execute for some
 reason (at least in sim)

---
 .../commands/SwerveDriveCommand.java          | 41 ++++---------------
 .../subsystems/drive/Drivetrain.java          | 22 ++++++++++
 2 files changed, 30 insertions(+), 33 deletions(-)

diff --git a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
index de3995cc..88c7b89d 100644
--- a/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/SwerveDriveCommand.java
@@ -2,11 +2,6 @@
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-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;
@@ -20,8 +15,6 @@ 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;
@@ -33,38 +26,20 @@ public void initialize() {
         xLimiter.reset(0);
         yLimiter.reset(0);
         rotLimiter.reset(0);
-        isFlipped =
-                DriverStation.getAlliance().isPresent()
-                        && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
+
     }
 
     @Override
     public void execute() {
-        double xPercent = xLimiter.calculate(-controller.getLeftY());
-        double yPercent = yLimiter.calculate(-controller.getLeftX());
-        double rotPercent = rotLimiter.calculate(-controller.getRightX());
-
-        // Apply deadband
-        double linearMagnitude = MathUtil.applyDeadband(Math.hypot(xPercent, yPercent), 0.1);
-        Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
-        double omega = MathUtil.applyDeadband(rotPercent, 0.1);
+        boolean isFlipped =
+            DriverStation.getAlliance().isPresent()
+                    && DriverStation.getAlliance().get() == DriverStation.Alliance.Red;
 
-        // Calculate new linear velocity
-        Translation2d linearVelocity =
-                new Pose2d(new Translation2d(), linearDirection)
-                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
+        double xPercent = MathUtil.applyDeadband(xLimiter.calculate(-controller.getLeftY()), 0.1);
+        double yPercent = MathUtil.applyDeadband(yLimiter.calculate(-controller.getLeftX()), 0.1);
+        double rotPercent = MathUtil.applyDeadband(rotLimiter.calculate(-controller.getRightX()), 0.1);
 
-        // Convert to field relative speeds & send command
-        drivetrain.runVelocity(
-                ChassisSpeeds.fromFieldRelativeSpeeds(
-                        linearVelocity.getX() * drivetrain.getMaxLinearSpeedMetersPerSec(),
-                        linearVelocity.getY() * drivetrain.getMaxLinearSpeedMetersPerSec(),
-                        omega * drivetrain.getMaxAngularSpeedRadPerSec(),
-                        isFlipped
-                                ? drivetrain.getRotation().plus(Rotation2d.fromDegrees(180))
-                                : drivetrain.getRotation()
-                )
-        );
+        drivetrain.drivePercent(xPercent, yPercent, rotPercent, isFlipped);
     }
 
     @Override
diff --git a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
index 373d06fa..e8e15ec1 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
+++ b/src/main/java/org/team1540/robot2024/subsystems/drive/Drivetrain.java
@@ -9,6 +9,7 @@
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+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.math.kinematics.SwerveDriveKinematics;
@@ -125,6 +126,27 @@ public void runVelocity(ChassisSpeeds speeds) {
         Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
     }
 
+    public void drivePercent(double xPercent, double yPercent, double rotPercent, boolean isFlipped) {
+        Rotation2d linearDirection = new Rotation2d(xPercent, yPercent);
+        double linearMagnitude = Math.hypot(xPercent, yPercent);
+
+        Translation2d linearVelocity =
+                new Pose2d(new Translation2d(), linearDirection)
+                        .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())).getTranslation();
+
+        // Convert to field relative
+        runVelocity(
+                ChassisSpeeds.fromFieldRelativeSpeeds(
+                        linearVelocity.getX() * getMaxLinearSpeedMetersPerSec(),
+                        linearVelocity.getY() * getMaxLinearSpeedMetersPerSec(),
+                        rotPercent * getMaxAngularSpeedRadPerSec(),
+                        isFlipped
+                                ? getRotation().plus(Rotation2d.fromDegrees(180))
+                                : getRotation()
+                )
+        );
+    }
+
     /**
      * Stops the drive.
      */